Cmd: Added smooth turn (not perfect) and "back start to center" commands

kbumsik · Mar 24, 2018 · b1465ab · b1465ab
1 parent 18a1d81
commit b1465ab
Show file tree

Hide file tree

Showing 3 changed files with 70 additions and 3 deletions.
diff --git a/firmware/blinky_wolfiemouse/main.cc b/firmware/blinky_wolfiemouse/main.cc
@@ -105,10 +105,9 @@ void task_blinky(void *pvParameters)
     // cmd_low_pid_and_go(&setpoints, NULL);
     // delay_ms(2000);
 
-    cmd_polling(CMD_L);
-    cmd_polling(CMD_F);
-    cmd_polling(CMD_F);
+    cmd_polling(CMD_S_L);
     cmd_polling(CMD_F);
+    cmd_polling(CMD_S_R);
     cmd_low_pid_reset_and_stop(NULL);
     /* Motor test running done */
 

diff --git a/firmware/src/board/wolfiemouse/cmd.h b/firmware/src/board/wolfiemouse/cmd.h
@@ -19,6 +19,7 @@ enum cmd_type {
     // low commands
     CMD_LOW_PID_AND_GO, CMD_LOW_PID_RESET_AND_STOP,
     // high level commands
+    CMD_BACK_TO_SART_CENTER, //< move forward from the back of the cell to the center of the cell
     CMD_F,      //< move forward 1 cell
     CMD_H_F,    //< move forward 1/2 cell
     CMD_L,      //< turn left in-place

diff --git a/firmware/src/board/wolfiemouse/thread_control_loop.c b/firmware/src/board/wolfiemouse/thread_control_loop.c
@@ -94,6 +94,19 @@ pid_value_t pid_rot_rotating_value = {
             .kd = 20
 };
 
+pid_value_t pid_tran_smooth_value = {
+        .kp = 35,
+        .ki = 5, // 0.01
+        .kd = 10
+};
+
+pid_value_t pid_rot_smooth_value = {
+            .kp = 30,
+            .ki = 0,
+            .kd = 20
+};
+
+
 // Range finder value
 volatile struct range_data range;
 
@@ -259,6 +272,24 @@ static void control_loop(void *pvParameters)
                         state.current_cmd = CMD_NOTHING;
                     break;
                     /* High level commands */
+                    case CMD_BACK_TO_SART_CENTER:
+                        pid_set_pid(&pid.tran, &pid_tran_forwarding_value);
+                        pid_set_pid(&pid.rot, &pid_rot_forwarding_value);
+
+                        pid_input_setpoint(&pid.tran, 18);
+                        pid_input_setpoint(&pid.rot, 0);
+
+                        target_step.left = MEASURE_STEPS_BACK_TO_START_CENTER;
+                        target_step.left += MEASURE_ENCODER_DEFAULT;
+                        target_step.right = MEASURE_STEPS_BACK_TO_START_CENTER;
+                        target_step.right += MEASURE_ENCODER_DEFAULT;
+
+                        state.left_wheel = WHEEL_FORWARD;
+                        state.right_wheel = WHEEL_FORWARD;
+
+                        system_enable_range_finder();
+                        system_start_driving();
+                    break;
                     case CMD_F:
                         pid_set_pid(&pid.tran, &pid_tran_forwarding_value);
                         pid_set_pid(&pid.rot, &pid_rot_forwarding_value);
@@ -335,6 +366,42 @@ static void control_loop(void *pvParameters)
                         system_disable_range_finder();  // MUST BE OFF
                         system_start_driving();
                     break;
+                    case CMD_S_L:
+                        pid_set_pid(&pid.tran, &pid_tran_smooth_value);
+                        pid_set_pid(&pid.rot, &pid_rot_smooth_value);
+
+                        pid_input_setpoint(&pid.tran, 15);
+                        pid_input_setpoint(&pid.rot, -30);
+
+                        target_step.left = MEASURE_STEPS_SMOOTH_L_LEFT;
+                        target_step.left += MEASURE_ENCODER_DEFAULT;
+                        target_step.right = MEASURE_STEPS_SMOOTH_L_RIGHT;
+                        target_step.right += MEASURE_ENCODER_DEFAULT;
+
+                        state.left_wheel = WHEEL_FORWARD;
+                        state.right_wheel = WHEEL_FORWARD;
+
+                        system_disable_range_finder();  // MUST BE OFF
+                        system_start_driving();
+                    break;
+                    case CMD_S_R:
+                        pid_set_pid(&pid.tran, &pid_tran_smooth_value);
+                        pid_set_pid(&pid.rot, &pid_rot_smooth_value);
+
+                        pid_input_setpoint(&pid.tran, 15);
+                        pid_input_setpoint(&pid.rot, 30);
+
+                        target_step.left = MEASURE_STEPS_SMOOTH_R_LEFT;
+                        target_step.left += MEASURE_ENCODER_DEFAULT;
+                        target_step.right = MEASURE_STEPS_SMOOTH_R_RIGHT;
+                        target_step.right += MEASURE_ENCODER_DEFAULT;
+
+                        state.left_wheel = WHEEL_FORWARD;
+                        state.right_wheel = WHEEL_FORWARD;
+
+                        system_disable_range_finder();  // MUST BE OFF
+                        system_start_driving();
+                    break;
                     default:
                         state.cmd_ready = 1;
                         state.current_cmd = CMD_NOTHING;