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

diff --git a/main.c b/main.c
index dd98345..ad0af05 100644
--- a/main.c
+++ b/main.c
@@ -1,4 +1,5 @@
 #include <aversive.h>
+#include <aversive/wait.h>
 
 struct servo {
 	uint8_t bit;
@@ -6,9 +7,13 @@ struct servo {
 };
 
 static struct servo servo_table[] = {
+	{
+		.bit = 2,
+		.command = 300,
+	},
 	{
 		.bit = 3,
-		.command = 0,
+		.command = 700,
 	},
 	{
 		.bit = 4,
@@ -37,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:
  *
@@ -152,11 +160,19 @@ int main(void)
 	uint8_t t, diff;
 
 	/* LED */
-	DDRB = 0x20;
+	DDRB = 0x02;
+
+#if 0 /* LED debug */
+	while (1) {
+		LED_ON();
+		wait_ms(100);
+		LED_OFF();
+		wait_ms(100);
+	}
+#endif
 
-	/* servo outputs */
-	DDRD = 0xf8;
-	DDRB |= 0x01;
+	/* servo outputs PD2-PD7 */
+	DDRD = 0xfc;
 
 	/* start timer1 at clk/1 (8Mhz) */
 	TCNT1 = 0;
@@ -171,7 +187,7 @@ int main(void)
 
 	sei();
 
-	bypass = 1;
+	bypass = 0;
 	while (1) {
 		t = TCNT0;
 		for (i = 0; i < NB_SERVO; i++) {
@@ -186,9 +202,11 @@ int main(void)
 		}
 		/* bypass mode */
 		while (bypass == 1) {
-			PORTD = ((PINC & 0x0f) << 3) | ((PINB & 0x01) << 7);
+			LED_ON();
+			PORTD = (PINC & 0x3f) << 2;
 			poll_spi();
 		}
+		LED_OFF();
 	}
 
 	return 0;