X-Git-Url: http://git.droids-corp.org/?p=protos%2Frc_servos.git;a=blobdiff_plain;f=main.c;h=ad0af0546c064a85832edb87ff925aa29c354c36;hp=c89b6373065d32ce23503e9ae27fab192238ca96;hb=f9f1d9f0311d8c84e8f2accac4ba5ea5f5481d6a;hpb=5cf087f7d27e0c7e565a1a7591020cedb6a9bcbf

diff --git a/main.c b/main.c
index c89b637..ad0af05 100644
--- a/main.c
+++ b/main.c
@@ -1,4 +1,5 @@
 #include <aversive.h>
+#include <aversive/wait.h>
 
 struct servo {
 	uint8_t bit;
@@ -7,16 +8,28 @@ struct servo {
 
 static struct servo servo_table[] = {
 	{
-		.bit = 0,
-		.command = 0,
+		.bit = 2,
+		.command = 300,
+	},
+	{
+		.bit = 3,
+		.command = 700,
 	},
 	{
-		.bit = 1,
+		.bit = 4,
 		.command = 512,
 	},
 	{
-		.bit = 2,
-		.command = 1023,
+		.bit = 5,
+		.command = 512,
+	},
+	{
+		.bit = 6,
+		.command = 512,
+	},
+	{
+		.bit = 7,
+		.command = 512,
 	},
 };
 #define NB_SERVO (sizeof(servo_table)/sizeof(*servo_table))
@@ -29,6 +42,9 @@ static volatile uint8_t rxidx;
 #define BYPASS_ENABLE 14
 #define BYPASS_DISABLE 15
 
+#define LED_ON() do { PORTB |= 0x02; } while(0)
+#define LED_OFF() do { PORTB &= ~0x02; } while(0)
+
 /*
  * SPI protocol:
  *
@@ -61,7 +77,7 @@ static volatile union {
 
 SIGNAL(TIMER1_COMPA_vect)
 {
-	PORTC = portval;
+	PORTD = portval;
 	TIMSK1 &= ~_BV(OCIE1A);
 	done = 1;
 }
@@ -144,10 +160,19 @@ int main(void)
 	uint8_t t, diff;
 
 	/* LED */
-	DDRB = 0x20;
+	DDRB = 0x02;
 
-	/* servo outputs */
-	DDRC = 0x7;
+#if 0 /* LED debug */
+	while (1) {
+		LED_ON();
+		wait_ms(100);
+		LED_OFF();
+		wait_ms(100);
+	}
+#endif
+
+	/* servo outputs PD2-PD7 */
+	DDRD = 0xfc;
 
 	/* start timer1 at clk/1 (8Mhz) */
 	TCNT1 = 0;
@@ -162,7 +187,7 @@ int main(void)
 
 	sei();
 
-	bypass = 1;
+	bypass = 0;
 	while (1) {
 		t = TCNT0;
 		for (i = 0; i < NB_SERVO; i++) {
@@ -177,8 +202,11 @@ int main(void)
 		}
 		/* bypass mode */
 		while (bypass == 1) {
-			PORTC = PORTB;
+			LED_ON();
+			PORTD = (PINC & 0x3f) << 2;
+			poll_spi();
 		}
+		LED_OFF();
 	}
 
 	return 0;