make servo pwm gen 0-750

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){
@@ -44,6 +45,14 @@ void atk_handle_packet(uint8_t *packet, uint8_t length){
 						//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
 						i ++;

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);
 

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

embedded/atkbbb/atkbbb/main.c

@@ -6,8 +6,40 @@
  */ 
 
 #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);
 		
@@ -15,46 +47,70 @@ void servo_set_pwm(uint16_t val){
 	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;
+		if(!fastModulo(tck, 4096)){
+			pin_toggle(&stlclk);
 		}
-			servo_set_pwm(pwm);
-			PORTE.DIRTGL = PIN3_bm;
-			PORTE.DIRTGL = PIN2_bm;
     }
 }
+
+
+ISR(USARTF0_RXC_vect){
+	uart_rxhandler(&up0);
 }
 
+ISR(USARTF0_DRE_vect){
+	uart_txhandler(&up0);
+}
\ No newline at end of file