main.cpp

00001 #include "mbed.h"
00002 
00003 #include "constants.h"
00004 
00005 #include "Button.h"
00006 #include "Screen.h"
00007 #include "motor.h"
00008 #include "EMGFilter.h"
00009 #include "motor_calibration.h"
00010 #include "forward_kinematics.h"
00011 #include "inverse_kinematics.h"
00012 #include "end_effector_control.h"
00013 #include "EMG_calibration.h"
00014 
00015 
00016 enum States {waiting, calib_motor, calib_bicep1, calib_bicep2, homing, operation, demo, failure}; // The possible states of the state machine
00017 
00018 
00019 
00020 // Controll directions for the demo controller.
00021 enum DebugControlDirection {debug_up, debug_down, debug_left, debug_right};
00022 
00023 
00024 Motor main_motor(D6, D7, D13, D12);
00025 Motor sec_motor(D5, D4, D10, D11);
00026 
00027 // For debugging purposes (with potmeter)
00028 AnalogIn potmeter1(A5); // Analoge input van potmeter 1 -> Motor 1
00029 AnalogIn potmeter2(A4); // Analoge input van potmeter 2 -> Motor 2
00030 
00031 EMGFilter emg_1(A0);    // Input for EMG signals
00032 EMGFilter emg_2(A1);    // Input for EMG signals
00033 
00034 States current_state;   // Defining the state we are currently in
00035 States last_state;      // To detect state changes.
00036 
00037 // Order of buttons: up_down, left_right, panic
00038 // D2, D3, D8
00039 Button ud_button(D2);       // Up-down
00040 Button lr_button(D3);       // Left-right
00041 Button p_button(D8);        // Panic (stop)
00042 
00043 Ticker button_ticker;       // Ticker for poll button to check whether the button is still pressed
00044 
00045 // LEDs for debugging purposes
00046 DigitalOut led_red(LED_RED);
00047 DigitalOut led_green(LED_GREEN);
00048 DigitalOut led_blue(LED_BLUE);
00049 
00050 // The last arguent (D6) is the reset pin.
00051 // The screen doesn't use it, but the library requires it
00052 // So pick a pin we don't use.
00053 // D14 and D15 are from and to the screen
00054 Screen screen(D14, D15, D9);        // Connects OLED screen (Adafruit)
00055 
00056 // Which direction the EMG (or for demo mode) will control the arm.
00057 // Up or down.
00058 // Left or right.
00059 bool control_goes_up = false;
00060 bool control_goes_right = false;
00061 
00062 
00063 void do_state_waiting()
00064 {
00065     if(last_state != current_state) {
00066         last_state = current_state;
00067         // State just changed to this one.
00068 
00069         led_green = 1;
00070         screen.clear_display();
00071         screen.display_state_name("Waiting");
00072         screen.get_screen_handle()->printf("    Press to start   ");
00073         screen.get_screen_handle()->printf("          |          ");
00074         screen.get_screen_handle()->printf("          V          ");
00075         screen.display();
00076     }
00077 
00078     if (ud_button.has_just_been_pressed()) {
00079         current_state = calib_motor;
00080     }
00081 }
00082 
00083 void do_state_calib_motor()
00084 {
00085     static double main_last_angle;
00086     static double sec_last_angle;
00087     static int main_iterations_not_moving;
00088     static int sec_iterations_not_moving;
00089     static bool main_is_calibrated;
00090     static bool sec_is_calibrated;
00091 
00092     if(last_state != current_state) {
00093         last_state = current_state;
00094         // State just changed to this one.
00095 
00096         led_green = 1;
00097         led_blue = 1;
00098         screen.clear_display();
00099         screen.display_state_name("Motor calibration");
00100 
00101         main_last_angle = -10;
00102         sec_last_angle = -10;
00103         main_iterations_not_moving = 0;
00104         sec_iterations_not_moving = 0;
00105         main_is_calibrated = false;
00106         sec_is_calibrated = false;
00107     }
00108 
00109     if (!main_is_calibrated) {
00110         led_green = 1;
00111         main_is_calibrated = calibrate_motor(main_motor, main_last_angle, main_iterations_not_moving);
00112         if (main_is_calibrated) {
00113             main_motor.define_current_angle_as_x_radians(main_motor_calibration_angle);
00114             //main_motor.set_target_angle(main_motor_calibration_angle - 0.2); // Give the arm some breathing space.
00115             led_green = 0;
00116         }
00117     }
00118     if (!sec_is_calibrated) {
00119         led_blue = 1;
00120         sec_is_calibrated = calibrate_motor(sec_motor, sec_last_angle, sec_iterations_not_moving);
00121         if (sec_is_calibrated) {
00122             sec_motor.define_current_angle_as_x_radians(sec_motor_calibration_angle); // -42 degrees.
00123             //main_motor.set_target_angle(sec_motor_calibration_angle + 0.2); // Give the arm some breathing space.
00124             led_blue = 0;
00125         }
00126     }
00127 
00128     screen.get_screen_handle()->setTextCursor(0, 8);
00129     screen.get_screen_handle()->printf("M: %i    \n", main_iterations_not_moving);
00130     screen.get_screen_handle()->printf("S: %i    \n", sec_iterations_not_moving);
00131     screen.display();
00132 
00133     if (main_is_calibrated && sec_is_calibrated) {
00134         current_state = homing;
00135     }
00136 }
00137 
00138 void do_state_homing()
00139 {
00140     // Position end-effector
00141     const double home_x = 0.6524; // Meters.
00142     const double home_y = 0.3409; // Meters
00143 
00144     double main_home;
00145     double sec_home;
00146 
00147     if(last_state != current_state) {
00148         last_state = current_state;
00149         // State just changed to this one.
00150         screen.clear_display();
00151         screen.display_state_name("Homing");
00152 
00153         inverse_kinematics(home_x, home_y, main_home, sec_home);
00154 
00155         main_motor.set_target_angle(main_home);
00156         sec_motor.set_target_angle(sec_home);
00157 
00158         screen.get_screen_handle()->setTextCursor(0, 8);
00159         screen.get_screen_handle()->printf("Ma: %.6f    \n", main_home);
00160         screen.get_screen_handle()->printf("Sa: %.6f    \n", sec_home);
00161         screen.display();
00162     }
00163 
00164     if (ud_button.has_just_been_pressed()) {
00165         current_state = calib_bicep1;
00166     }
00167     if (lr_button.has_just_been_pressed()) {
00168         current_state = demo;                   // Back-up for if EMG does not work
00169     }
00170 }
00171 
00172 void do_state_calib_bicep1()
00173 {
00174     static EMG_calibration calibration = EMG_calibration(&screen, &emg_1);
00175 
00176     if(last_state != current_state) {
00177         last_state = current_state;
00178         // State just changed to this one.
00179         screen.clear_display();
00180         screen.display_state_name("EMG 1 calibration");
00181 
00182         calibration.start();        // Starts calibration of bicep 1
00183     }
00184 
00185     if (ud_button.has_just_been_pressed() && calibration.is_calibrated()) {
00186         current_state = calib_bicep2;
00187     }
00188 }
00189 
00190 void do_state_calib_bicep2()
00191 {
00192     static EMG_calibration calibration = EMG_calibration(&screen, &emg_2);
00193 
00194     if(last_state != current_state) {
00195         last_state = current_state;
00196         // State just changed to this one.
00197         screen.clear_display();
00198         screen.display_state_name("EMG 2 calibration");
00199 
00200         calibration.start();
00201     }
00202 
00203     if (ud_button.has_just_been_pressed() && calibration.is_calibrated()) {
00204         current_state = operation;
00205     }
00206 }
00207 
00208 void do_state_operation()
00209 {
00210     static const double max_speed = 0.01;       // 1 cm per iteration (as long as EMG signals are received, setpoint is set 1 cm away)
00211     static double speed_x;
00212     static double speed_y;
00213     
00214     static bool last_state_1;
00215     static bool last_state_2;
00216 
00217     if(last_state != current_state) {
00218         last_state = current_state;
00219         // State just changed to this one.
00220         screen.clear_display();
00221         screen.display_state_name("Normal operation");
00222 
00223         control_goes_up = true;
00224         control_goes_right = true;
00225         
00226         speed_x = 0;
00227         speed_y = 0;
00228 
00229         screen.display_up_down_arrow(control_goes_up);
00230         screen.display_left_right_arrow(control_goes_right);
00231         
00232         last_state_1 = false;
00233         last_state_2 = false;
00234     }
00235     
00236     bool emg_1_state = emg_1.get_is_envelope_over_threshold();
00237     bool emg_2_state = emg_2.get_is_envelope_over_threshold();
00238     
00239     if (emg_1_state) {
00240         led_green = 0;
00241 
00242         if (control_goes_right) {
00243             speed_x = max_speed;
00244         } else {
00245             speed_x = -max_speed;
00246         }
00247     } else {
00248         led_green = 1;
00249         speed_x = 0;
00250     }
00251     
00252     if (emg_2_state) {
00253         led_blue = 0;
00254         if (control_goes_up) {
00255             speed_y = max_speed;
00256         } else {
00257             speed_y = -max_speed;
00258         }
00259     } else {
00260         led_blue = 1;
00261         speed_y = 0;
00262     }
00263     
00264     last_state_1 = emg_1_state;
00265     last_state_2 = emg_2_state;
00266   
00267     if (speed_x || speed_y) {               // Ensures that if the end-effector is moved externally, the motors will resist (setpoint changes)
00268         
00269         double main_cur_angle = main_motor.get_current_angle();
00270         double sec_cur_angle = sec_motor.get_current_angle();
00271     
00272         double main_target, sec_target;
00273     
00274         end_effector_control(speed_x, speed_y, main_cur_angle, sec_cur_angle, main_target, sec_target);
00275     
00276         main_motor.set_target_angle(main_target);
00277         sec_motor.set_target_angle(sec_target);
00278     }
00279 
00280 
00281     if (ud_button.has_just_been_pressed()) {
00282         control_goes_up = !control_goes_up;
00283         screen.display_up_down_arrow(control_goes_up);
00284     }
00285 
00286     if (lr_button.has_just_been_pressed()) {
00287         control_goes_right = !control_goes_right;
00288         screen.display_left_right_arrow(control_goes_right);
00289     }
00290 }
00291 
00292 void do_state_demo()
00293 {
00294     static DebugControlDirection control_direction;
00295     static const double max_speed = 0.01;
00296     static double speed_x;
00297     static double speed_y;
00298 
00299     if(last_state != current_state) {
00300         last_state = current_state;
00301         // State just changed.
00302         // Update the display.
00303         led_red = 1;
00304         led_green = 0;
00305         led_blue = 1;
00306         screen.clear_display();
00307         screen.display_state_name("Demo mode!");
00308 
00309         control_direction = debug_up;
00310 
00311         speed_x = 0;
00312         speed_y = 0;
00313 
00314         screen.display_up_down_arrow(control_goes_up);
00315         screen.display_left_right_arrow(control_goes_right);
00316     }
00317 
00318     if (lr_button.has_just_been_pressed()) {
00319         switch (control_direction) {
00320             case debug_up:
00321                 control_direction = debug_right;
00322                 speed_x = max_speed;
00323                 speed_y = 0;
00324                 break;
00325             case debug_right:
00326                 control_direction = debug_down;
00327                 speed_x = 0;
00328                 speed_y = -max_speed;
00329                 break;
00330             case debug_down:
00331                 control_direction = debug_left;
00332                 speed_x = -max_speed;
00333                 speed_y = 0;
00334                 break;
00335             case debug_left:
00336                 control_direction = debug_up;
00337                 speed_x = 0;
00338                 speed_y = max_speed;
00339                 break;
00340         }
00341     }
00342 
00343     if (ud_button.is_pressed()) {
00344 
00345         led_blue = 0;
00346 
00347 
00348         double main_cur_angle = main_motor.get_current_angle();
00349         double sec_cur_angle = sec_motor.get_current_angle();
00350 
00351         double main_target, sec_target;
00352 
00353         end_effector_control(speed_x, speed_y, main_cur_angle, sec_cur_angle, main_target, sec_target);
00354 
00355         main_motor.set_target_angle(main_target);
00356         sec_motor.set_target_angle(sec_target);
00357 
00358         screen.get_screen_handle()->setTextCursor(0, 0);
00359         screen.get_screen_handle()->printf("M_a: %.6f    \n", main_cur_angle);
00360         screen.get_screen_handle()->printf("S_a: %.6f    \n", sec_cur_angle);
00361         screen.get_screen_handle()->printf("Vx:  %.6f \n", main_target);
00362         screen.get_screen_handle()->printf("Vy:  %.6f ", sec_target);
00363         screen.display();
00364     }
00365 }
00366 
00367 void do_state_failure()
00368 {
00369     if(last_state != current_state) {
00370         last_state = current_state;
00371         // State just changed.
00372         // Update the display.
00373         led_red = 0;
00374         led_green = 1;
00375         led_blue = 1;
00376         screen.clear_display();
00377         screen.display_state_name("STOP");
00378     }
00379 
00380     // Stop the motors!
00381     main_motor.stop();
00382     sec_motor.stop();
00383 }
00384 
00385 
00386 void main_loop()
00387 {
00388     // Update buttons
00389     ud_button.update();
00390     lr_button.update();
00391     p_button.update();
00392 
00393     switch (current_state) {
00394         case waiting:
00395             do_state_waiting();
00396             break;
00397         case calib_motor:
00398             do_state_calib_motor();
00399             break;
00400         case calib_bicep1:
00401             do_state_calib_bicep1();
00402             break;
00403         case calib_bicep2:
00404             do_state_calib_bicep2();
00405             break;
00406         case homing:
00407             do_state_homing();
00408             break;
00409         case operation:
00410             do_state_operation();
00411             break;
00412         case demo:
00413             do_state_demo();
00414             break;
00415         case failure:
00416             do_state_failure();
00417             break;
00418     }
00419 
00420     // Check if the panic button was pressed.
00421     // Doesn't matter in which state we are, we need to go to failure.
00422     if (p_button.is_pressed()) {
00423         current_state = failure;
00424     }
00425 }
00426 
00427 void poll_buttons()
00428 {
00429     // We need to poll the pins periodically.
00430     // Normally one would use rise and fall interrupts, so this wouldn't be
00431     // needed. But the buttons we use generate so much chatter that
00432     // sometimes a rising or a falling edge doesn't get registered.
00433     // With all the confusion that accompanies it.
00434     ud_button.poll_pin();
00435     lr_button.poll_pin();
00436     p_button.poll_pin();
00437 }
00438 
00439 int main()
00440 {
00441     led_red = 1;
00442     led_green = 1;
00443     led_blue = 1;
00444 
00445     screen.clear_display();
00446 
00447     main_motor.set_pid_k_values(Kp, Ki, Kd);
00448     sec_motor.set_pid_k_values(Kp, Ki, Kd);
00449 
00450     // One of the motors is reversed in the electronics.
00451     // This is fixed in the motor controll board, so we have to account
00452     // for it in software.
00453     // Extra reduction ratio is needed to give the "real" angles that the gears need to make (ratio of both gears is different)
00454     main_motor.set_extra_reduction_ratio(-main_gear_ratio);
00455     sec_motor.set_extra_reduction_ratio(sec_gear_ratio);
00456 
00457     // Set the maximum speed for both motors.
00458     main_motor.set_max_speed(0.5);
00459     sec_motor.set_max_speed(0.5);
00460 
00461     // Start the motor controller at the desired frequency.
00462     main_motor.start(pid_period);
00463     sec_motor.start(pid_period);
00464 
00465     // Start in the waiting state.
00466     current_state = waiting;
00467     // Pretend we come from the operation state.
00468     // So that the waiting state knows it just got started.
00469     last_state = operation;
00470 
00471     emg_1.start(emg_period);
00472     emg_2.start(emg_period);
00473 
00474     // Start the button polling ticker.
00475     button_ticker.attach(&poll_buttons, button_poll_interval);
00476 
00477     while (true) {
00478         main_loop();        // In while(true) to keep from stalling the interrupts (around 0.01 times per seconds)
00479 
00480         wait(main_loop_wait_time);
00481     }
00482 }