summaryrefslogtreecommitdiff
path: root/n/asserv/src/main.c
diff options
context:
space:
mode:
Diffstat (limited to 'n/asserv/src/main.c')
-rw-r--r--n/asserv/src/main.c79
1 files changed, 39 insertions, 40 deletions
diff --git a/n/asserv/src/main.c b/n/asserv/src/main.c
index e6c2303..ef607b3 100644
--- a/n/asserv/src/main.c
+++ b/n/asserv/src/main.c
@@ -32,6 +32,12 @@
#include <avr/io.h>
#include <avr/signal.h>
+/** Motor mode :
+ * 0 - pwm setup.
+ * 1 - speed control.
+ * 2 - position control. */
+int8_t motor_mode;
+
#include "pwm.c"
#include "timer.c"
#include "counter.c"
@@ -43,12 +49,6 @@
/** Use Taz. */
uint8_t motor_taz;
-/** Motor mode :
- * 0 - pwm setup.
- * 1 - speed control.
- * 2 - position control. */
-int8_t motor_mode;
-
/** Main loop counter. */
uint8_t motor_loop_cpt;
@@ -97,6 +97,7 @@ int
main (void)
{
DDRD = 0x60;
+ PORTA = _BV (0) | _BV (7);
pwm_init ();
timer_init ();
counter_init ();
@@ -140,11 +141,7 @@ main_loop (void)
motor_timer_2 = timer_read ();
/* Speed control. */
if (motor_mode >= 1)
- {
speed_update ();
- speed_compute_left_pwm ();
- speed_compute_right_pwm ();
- }
motor_timer_1 = timer_read ();
/* Pwm setup. */
pwm_update ();
@@ -229,40 +226,29 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
case c ('l', 2):
/* Linear move.
* - w: distance (negative to go backward). */
- motor_mode = 2;
- goto_mode = 0;
- goto_sign = args[0] >> 7;
- goto_d = v8_to_v16 (args[0], args[1]);
- if (goto_sign) goto_d = -goto_d;
- goto_x = postrack_x;
- goto_y = postrack_y;
- goto_finish = 0;
+ goto_linear (v8_to_v16 (args[0], args[1]));
break;
case c ('a', 1):
/* Angular move.
* - b: absolute angle. */
- motor_mode = 2;
- goto_mode = 1;
- goto_a = args[0];
- goto_finish = 0;
+ goto_angular (args[0]);
break;
case c ('g', 8):
/* Go to position move.
* - d: f24.8, x position.
* - d: f24.8, y position. */
- motor_mode = 2;
- goto_mode = 2;
- goto_x = v8_to_v32 (args[0], args[1], args[2], args[3]);
- goto_y = v8_to_v32 (args[4], args[5], args[6], args[7]);
- goto_finish = 0;
+ goto_position (v8_to_v32 (args[0], args[1], args[2], args[3]),
+ v8_to_v32 (args[4], args[5], args[6], args[7]));
+ break;
+ case c ('p', 4):
+ /* Counter mode. */
+ goto_counter (v8_to_v16 (args[0], args[1]),
+ v8_to_v16 (args[2], args[3]));
break;
case c ('f', 1):
/* Fuck the wall move.
* - b: speed. */
- motor_mode = 2;
- goto_mode = 10;
- goto_s = args[0];
- goto_finish = 0;
+ goto_ftw (args[0]);
break;
case c ('F', 1):
/* Setup finish flag.
@@ -365,13 +351,25 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
goto_eps = v8_to_v32 (args[1], args[2], args[3], args[4]);
break;
case c ('p', 3):
- speed_kp = v8_to_v16 (args[1], args[2]);
+ speed_left_kp = speed_right_kp = v8_to_v16 (args[1], args[2]);
+ break;
+ case c ('p', 5):
+ speed_left_kp = v8_to_v16 (args[1], args[2]);
+ speed_right_kp = v8_to_v16 (args[3], args[4]);
break;
case c ('i', 3):
- speed_ki = v8_to_v16 (args[1], args[2]);
+ speed_left_ki = speed_right_ki = v8_to_v16 (args[1], args[2]);
+ break;
+ case c ('i', 5):
+ speed_left_ki = v8_to_v16 (args[1], args[2]);
+ speed_right_ki = v8_to_v16 (args[3], args[4]);
break;
case c ('d', 3):
- speed_kd = v8_to_v16 (args[1], args[2]);
+ speed_left_kd = speed_right_kd = v8_to_v16 (args[1], args[2]);
+ break;
+ case c ('d', 5):
+ speed_left_kd = v8_to_v16 (args[1], args[2]);
+ speed_right_kd = v8_to_v16 (args[3], args[4]);
break;
case c ('E', 3):
speed_e_sat = v8_to_v16 (args[1], args[2]);
@@ -386,9 +384,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
/* Set acceleration. */
speed_acc_cpt = speed_acc = args[1];
break;
- case c ('m', 2):
+ case c ('m', 3):
/* Set maximum speed for automatic movements. */
- speed_max = args[1];
+ speed_max_l = args[1];
+ speed_max_a = args[2];
break;
case c ('w', 3):
/* Set PWM direction.
@@ -411,10 +410,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
proto_send1b ('E', EEPROM_KEY);
proto_send1w ('f', postrack_footing);
proto_send1b ('a', speed_acc);
- proto_send1b ('m', speed_max);
- proto_send1w ('p', speed_kp);
- proto_send1w ('i', speed_ki);
- proto_send1w ('d', speed_kd);
+ proto_send2b ('m', speed_max_l, speed_max_a);
+ proto_send2w ('p', speed_left_kp, speed_right_kp);
+ proto_send2w ('i', speed_left_ki, speed_right_ki);
+ proto_send2w ('d', speed_left_kd, speed_right_kd);
proto_send1w ('E', speed_e_sat);
proto_send1w ('I', speed_int_sat);
proto_send1b ('s', speed_ds);