Commits

Trammell Hudson committed 9a99c95 Draft

proportional dance of death works on axis 1

Comments (0)

Files changed (1)

 		out(LED, 0);
 }
 
-static uint16_t output;
 
 void
 output_drive(
 }
 
 
+static uint8_t output_enabled;
+
 void
 output_enable(
 	uint8_t val
 )
 {
-	if (val)
-	{
-		// pull the enable down to ground; output is already 0
-		ddr(SERVO_ENABLE, 1);
-		led(1);
-		return;
-	}
+	output_enabled = val;
 
-	// let the enable float and bring the ref to neutral
-	ddr(SERVO_ENABLE, 0);
+	// if disabling, let the enable pin float.  Otherwise it will
+	// be pulled to ground.
+	ddr(SERVO_ENABLE, val);
+
+	// Generate zero-drive commands, always, just in case
 	for (int i = 0 ; i < 6 ; i++)
 		output_drive(i, 0);
-	led(0);
+
+	// Indicate our drive status on the LED.
+	led(val);
 }
 
 
 /** Position counters for each axis */
 static int16_t positions[6];
+static int16_t commands[6];
 
 static inline void
 quad_decode(
 	int16_t p_in
 )
 {
-	uint16_t p = p_in;
+	int16_t p = p_in;
 	if (p == 0)
 	{
 		*buf++ = ' ';
 }
 
 
+static void
+position_command(
+	uint8_t i,
+	int16_t command
+)
+{
+	commands[i] = command;
+}
+
+
+static void
+position_loop(void)
+{
+	if (!output_enabled)
+		return;
+
+	const int i = 1;
+	const int gain = 16;
+
+	int16_t error = commands[i] - positions[i];
+/*
+	if (-128 < error)
+		error = -128;
+	else
+	if (+127 > error)
+		error = 127;
+*/
+
+	int16_t output = (error * gain) / 256;
+	if (output < -128)
+		output = -128;
+	else
+	if (output > 127)
+		output = 127;
+		
+	output_drive(i, output);
+}
+
+
 int
 main(void)
 {
 	out(SERVO_QY_5, 1);
 	out(SERVO_QY_6, 1);
 
+	// try turning off pullups
+	out(SERVO_QX_1, 0);
+	out(SERVO_QX_2, 0);
+	out(SERVO_QX_3, 0);
+	out(SERVO_QX_4, 0);
+	out(SERVO_QX_5, 0);
+	out(SERVO_QX_6, 0);
+
+	out(SERVO_QY_1, 0);
+	out(SERVO_QY_2, 0);
+	out(SERVO_QY_3, 0);
+	out(SERVO_QY_4, 0);
+	out(SERVO_QY_5, 0);
+	out(SERVO_QY_6, 0);
+
 	cbi(EICRA, ISC01); // 01 == any edge
 	sbi(EICRA, ISC00);
 	sbi(EIMSK, INT0);
 	usb_serial_flush_input();
 
 	send_str(PSTR("servo driver\r\n"));
-	uint8_t val = 0;
+	uint16_t val = 0;
 	uint8_t sign = 1;
 
 	while (1)
 		int c = usb_serial_getchar();
 		if (c == -1)
 		{
+			position_loop();
+
 			if (bit_is_clear(ADCSRA, ADIF))
 				continue;
 
 			uint16_t val = ADC;
 			sbi(ADCSRA, ADIF);
 			sbi(ADCSRA, ADSC);
+			(void) val;
 
 			char buf[80];
 			uint8_t off = 0;
 			off += print_position(buf+off, positions[1]);
 			buf[off++] = ' ';
 			off += print_position(buf+off, positions[2]);
+#if 0 // don't have the other three motors yet
 			buf[off++] = ' ';
 			off += print_position(buf+off, positions[3]);
 			buf[off++] = ' ';
 			off += print_position(buf+off, positions[4]);
 			buf[off++] = ' ';
 			off += print_position(buf+off, positions[5]);
+#endif
 
 			buf[off++] = '\r';
 			buf[off++] = '\n';
 
 		if (c == '!')
 		{
+			// copy the current positions into the commands
+			// this avoids any jumps
+			for (int i = 0 ;i < 6 ; i++)
+			{
+				position_command(i, positions[i]);
+				output_drive(i, 0);
+			}
+
 			output_enable(1);
 			continue;
 		}
 		if (c == '\r')
 		{
 			send_str(PSTR("+\r\n"));
-			output_drive(1, sign ? val : -val);
+			position_command(1, sign ? val : -val);
 			val = 0;
 			sign = 1;
 			continue;