From de05a9b0aaeabf429dabe16b09cab14c84a42902 Mon Sep 17 00:00:00 2001
From: Jake <jake.read@cba.mit.edu>
Date: Thu, 1 Nov 2018 15:14:24 -0400
Subject: [PATCH] make servo pwm gen 0-750
---
embedded/atkbbb/atkbbb/atkhandler.c | 9 +++
embedded/atkbbb/atkbbb/atkhandler.h | 3 +
embedded/atkbbb/atkbbb/hardware.h | 79 ++----------------------
embedded/atkbbb/atkbbb/main.c | 96 +++++++++++++++++++++++------
4 files changed, 92 insertions(+), 95 deletions(-)
diff --git a/embedded/atkbbb/atkbbb/atkhandler.c b/embedded/atkbbb/atkbbb/atkhandler.c
index e97d17c..7834ebd 100644
--- a/embedded/atkbbb/atkbbb/atkhandler.c
+++ b/embedded/atkbbb/atkbbb/atkhandler.c
@@ -17,6 +17,7 @@ void atk_handle_packet(uint8_t *packet, uint8_t length){
int i = 0;
int atk_handler_state = ATK_HANDLER_OUTSIDE;
uint8_t testReply[4] = {127, 12, 24, 48};
+ uint8_t servoReply[5] = {141, 0, 1, 2, 3};
while(i < length){ // prep for the messy double switch :|
switch (atk_handler_state){
@@ -43,6 +44,14 @@ void atk_handle_packet(uint8_t *packet, uint8_t length){
case DELIM_KEY_TRAPEZOID:
//pin_toggle(&stlerr);
break;
+
+ case DELIM_KEY_SERVOVAL:
+ ; // empty statement for C error
+ uint32_t servoval = (uint32_t)(packet[i + 1] << 24) | (uint32_t)(packet[i + 2] << 16) | (uint32_t)(packet[i + 3] << 8) | (uint32_t)(packet[i + 4]);
+ i += 5;
+ atk_reply_packet(packet, servoReply, 5);
+ set_servo((uint16_t)servoval);
+ break;
default:
// probably an error
diff --git a/embedded/atkbbb/atkbbb/atkhandler.h b/embedded/atkbbb/atkbbb/atkhandler.h
index d676f9a..269f684 100644
--- a/embedded/atkbbb/atkbbb/atkhandler.h
+++ b/embedded/atkbbb/atkbbb/atkhandler.h
@@ -18,6 +18,9 @@
#define DELIM_KEY_RESET 128
#define DELIM_KEY_TRAPEZOID 131
+#define DELIM_KEY_WAIT 132
+
+#define DELIM_KEY_SERVOVAL 141
void atk_handle_packet(uint8_t *packet, uint8_t length);
diff --git a/embedded/atkbbb/atkbbb/hardware.h b/embedded/atkbbb/atkbbb/hardware.h
index e95fe05..10d097e 100644
--- a/embedded/atkbbb/atkbbb/hardware.h
+++ b/embedded/atkbbb/atkbbb/hardware.h
@@ -19,7 +19,7 @@
// A: 11 B: 0 - 4uS bit, 250kBaud
#define SYSTEM_BAUDA 11
#define SYSTEM_BAUDB 0
-#define SYSTEM_NUM_UPS 7
+#define SYSTEM_NUM_UPS 1
pin_t stlclk;
pin_t stlerr;
@@ -36,81 +36,10 @@ pin_t up0txled;
atkport_t atkp0;
-// UP1
-
-ringbuffer_t up1rbrx;
-ringbuffer_t up1rbtx;
-
-uartport_t up1;
-
-pin_t up1rxled;
-pin_t up1txled;
-
-atkport_t atkp1;
-
-// UP2
-
-ringbuffer_t up2rbrx;
-ringbuffer_t up2rbtx;
-
-uartport_t up2;
-
-pin_t up2rxled;
-pin_t up2txled;
-
-atkport_t atkp2;
-
-// UP3
-
-ringbuffer_t up3rbrx;
-ringbuffer_t up3rbtx;
-
-uartport_t up3;
-
-pin_t up3rxled;
-pin_t up3txled;
-
-atkport_t atkp3;
-
-// UP4
-
-ringbuffer_t up4rbrx;
-ringbuffer_t up4rbtx;
-
-uartport_t up4;
-
-pin_t up4rxled;
-pin_t up4txled;
-
-atkport_t atkp4;
-
-// UP 5
-
-
-ringbuffer_t up5rbrx;
-ringbuffer_t up5rbtx;
-
-uartport_t up5;
-
-pin_t up5rxled;
-pin_t up5txled;
-
-atkport_t atkp5;
-
-// UPS
-
-// UP USB (UP6)
-
-ringbuffer_t upUrbrx;
-ringbuffer_t upUrbtx;
-
-uartport_t upU;
-
-pin_t upUrxled; // don't exist
-pin_t upUtxled;
+uartport_t *ups[SYSTEM_NUM_UPS];
-atkport_t atkp6;
+// MAIN FCNS
-uartport_t *ups[SYSTEM_NUM_UPS];
+void set_servo(uint16_t val);
#endif /* HARDWARE_H_ */
\ No newline at end of file
diff --git a/embedded/atkbbb/atkbbb/main.c b/embedded/atkbbb/atkbbb/main.c
index b4b1e01..7b47ccb 100644
--- a/embedded/atkbbb/atkbbb/main.c
+++ b/embedded/atkbbb/atkbbb/main.c
@@ -6,55 +6,111 @@
*/
#include <avr/io.h>
+#include <avr/interrupt.h>
+#include "hardware.h"
+#include "fastmath.h"
+#include "pin.h"
-void servo_set_pwm(uint16_t val){
+void clock_init(void){
+ OSC.XOSCCTRL = OSC_XOSCSEL_XTAL_256CLK_gc | OSC_FRQRANGE_12TO16_gc; // select external source
+ OSC.CTRL = OSC_XOSCEN_bm; // enable external source
+ while(!(OSC.STATUS & OSC_XOSCRDY_bm)); // wait for external
+ OSC.PLLCTRL = OSC_PLLSRC_XOSC_gc | OSC_PLLFAC0_bm | OSC_PLLFAC1_bm; // select external osc for pll, do pll = source * 3
+ //OSC.PLLCTRL = OSC_PLLSRC_XOSC_gc | OSC_PLLFAC1_bm; // pll = source * 2 for 32MHz std clock
+ OSC.CTRL |= OSC_PLLEN_bm; // enable PLL
+ while (!(OSC.STATUS & OSC_PLLRDY_bm)); // wait for PLL to be ready
+ CCP = CCP_IOREG_gc; // enable protected register change
+ CLK.CTRL = CLK_SCLKSEL_PLL_gc; // switch to PLL for main clock
+}
+
+void uarts_init(void){
+ rb_init(&up0rbrx);
+ rb_init(&up0rbtx);
+ pin_init(&up0rxled, &PORTF, PIN0_bm, 0, 1);
+ pin_init(&up0txled, &PORTF, PIN1_bm, 1, 1);
+ uart_init(&up0, &USARTF0, &PORTF, PIN2_bm, PIN3_bm, &up0rbrx, &up0rbtx, &up0rxled, &up0txled);
+
+ uart_start(&up0, SYSTEM_BAUDA, SYSTEM_BAUDB);
+
+ ups[0] = &up0;
+}
+
+void atkps_init(void){
+ atkport_init(&atkp0, 0, &up0);
+}
+
+void set_pwm_cca(uint16_t val){
uint8_t vall = (uint8_t) val;
uint8_t valh = (uint8_t) (val >> 8);
-
+
TCC0.CCABUFL = vall;
TCC0.CCABUFH = valh;
}
-void servo_pwm_begin(){
+void servo_pwm_begin(void){
PORTC.DIRSET = PIN0_bm;
- TCC0.CTRLA = TC_CLKSEL_DIV8_gc;
+ TCC0.CTRLA = TC_CLKSEL_DIV64_gc;
TCC0.CTRLB = TC_WGMODE_SINGLESLOPE_gc | (1 << 4);
- uint16_t per = 1024;
+ uint16_t per = 15000;
uint8_t perl = (uint8_t) per;
uint8_t perh = (uint8_t) (per >> 8);
TCC0.PERBUFL = perl;
TCC0.PERBUFH = perh;
- servo_set_pwm(512);
+ // 750: 1ms
+ // 1500: 2ms
+ set_pwm_cca(750);
+}
+
+void set_servo(uint16_t val){
+ // 0 -> 750 = signal 1000us -> 2000us duty on 20ms period
+ if (val > 1500){
+ val = 1500;
+ }
+ set_pwm_cca(750 + val);
}
int main(void)
{
- servo_pwm_begin();
+ // start clock
+ clock_init();
+
+ // start networking hardware
+ uarts_init();
+ atkps_init();
+
+ // allow interrupts (and set handlers below)
+ sei();
+ PMIC.CTRL |= PMIC_LOLVLEN_bm | PMIC_MEDLVLEN_bm | PMIC_HILVLEN_bm;
+
+ // lights
+ pin_init(&stlclk, &PORTE, PIN2_bm, 2, 1);
+ pin_init(&stlerr, &PORTE, PIN3_bm, 3, 1);
+ pin_set(&stlerr);
- // 'pin' 40 / port e - pin 3
- PORTE.DIRSET = PIN3_bm;
- PORTE.DIRSET = PIN2_bm;
+ // servo begin
+ servo_pwm_begin();
uint16_t tck = 0;
- uint16_t pwm = 256;
while (1)
{
+ atkport_scan(&atkp0, 2);
tck ++;
-
- if(!(tck % 800)){
- pwm ++;
- if(pwm > 512){
- pwm = 256;
- }
- servo_set_pwm(pwm);
- PORTE.DIRTGL = PIN3_bm;
- PORTE.DIRTGL = PIN2_bm;
+ if(!fastModulo(tck, 4096)){
+ pin_toggle(&stlclk);
}
}
}
+
+ISR(USARTF0_RXC_vect){
+ uart_rxhandler(&up0);
+}
+
+ISR(USARTF0_DRE_vect){
+ uart_txhandler(&up0);
+}
\ No newline at end of file
--
GitLab