Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: HIDScope mbed MODSERIAL QEI
main.cpp@20:c3be3bb3b515, 2017-11-03 (annotated)
- Committer:
- john111222333
- Date:
- Fri Nov 03 09:08:35 2017 +0000
- Revision:
- 20:c3be3bb3b515
- Parent:
- 19:b51b84a1f195
- Child:
- 21:59431788a42d
working :straight" lines
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Alex_Kyrl | 6:452e301a105a | 1 | #include "EMG.h" |
Alex_Kyrl | 6:452e301a105a | 2 | #include "Motor.h" |
poephoofd | 0:2d9dae739559 | 3 | #include "HIDScope.h" |
poephoofd | 2:a08bff88216d | 4 | #include "MODSERIAL.h" |
Alex_Kyrl | 8:0b7925095416 | 5 | #include "iostream" |
Alex_Kyrl | 9:22d79a4a0324 | 6 | DigitalIn a(D3); //buttons for testing |
Alex_Kyrl | 9:22d79a4a0324 | 7 | DigitalIn b(D2); |
john111222333 | 20:c3be3bb3b515 | 8 | //AnalogIn pot1(A0); //potmeters for testing |
john111222333 | 20:c3be3bb3b515 | 9 | //AnalogIn pot2(A1); |
Alex_Kyrl | 9:22d79a4a0324 | 10 | double cont = 0 ; |
Alex_Kyrl | 9:22d79a4a0324 | 11 | |
Alex_Kyrl | 11:dd1976534a03 | 12 | HIDScope scope(6); // 4 channels of data |
poephoofd | 0:2d9dae739559 | 13 | Ticker MainTicker; |
poephoofd | 0:2d9dae739559 | 14 | MODSERIAL pc(USBTX, USBRX); |
poephoofd | 0:2d9dae739559 | 15 | |
Alex_Kyrl | 8:0b7925095416 | 16 | /*****************************************************************/ |
john111222333 | 14:f561498eee28 | 17 | //Initialize Analog EMG inputs: |
Alex_Kyrl | 9:22d79a4a0324 | 18 | EMG EMG_bi_r(A0); // Move the endpoint to the right (plus direction) |
Alex_Kyrl | 9:22d79a4a0324 | 19 | EMG EMG_bi_l(A1); // Move the endpoint to the left (minus direction) |
Alex_Kyrl | 9:22d79a4a0324 | 20 | EMG EMG_tri_r(A2); // Move the endpoint forward (plus direction) |
Alex_Kyrl | 9:22d79a4a0324 | 21 | EMG EMG_tri_l(A3); // Move the endpoint backward (minus direction) |
Alex_Kyrl | 8:0b7925095416 | 22 | |
Alex_Kyrl | 8:0b7925095416 | 23 | /****************************************************/ |
Alex_Kyrl | 9:22d79a4a0324 | 24 | //Initialise Motors: |
john111222333 | 20:c3be3bb3b515 | 25 | int angle_start1 = 100;//51.3676; |
john111222333 | 20:c3be3bb3b515 | 26 | int angle_start2 = 100;//140.2431; |
Alex_Kyrl | 8:0b7925095416 | 27 | |
john111222333 | 20:c3be3bb3b515 | 28 | Motor motor1(D13 , D12 , D7 , D6 , 50000 , 90 , 0.5 , 0 , angle_start1 , 0.05 , 0.01, 0.00000); |
john111222333 | 20:c3be3bb3b515 | 29 | Motor motor2(D11 , D10 , D4 , D5 , 50000 , 50 , 0.6 , 0 , angle_start2 , 0.07 , 0.01 , 0.00000 ); |
john111222333 | 16:a2a73d57d556 | 30 | |
poephoofd | 0:2d9dae739559 | 31 | |
Alex_Kyrl | 8:0b7925095416 | 32 | /*****************************************************/ |
Alex_Kyrl | 8:0b7925095416 | 33 | // Set control signals: |
poephoofd | 0:2d9dae739559 | 34 | |
Alex_Kyrl | 9:22d79a4a0324 | 35 | //x direction is the righ/left movement |
Alex_Kyrl | 9:22d79a4a0324 | 36 | //y direction is forward/backward movement |
Alex_Kyrl | 9:22d79a4a0324 | 37 | |
Alex_Kyrl | 9:22d79a4a0324 | 38 | double get_X_control_signal(){ |
Alex_Kyrl | 9:22d79a4a0324 | 39 | double emg_right = EMG_bi_r.filter(); |
Alex_Kyrl | 9:22d79a4a0324 | 40 | double emg_left = EMG_bi_l.filter(); |
john111222333 | 20:c3be3bb3b515 | 41 | //scope.set(0 ,emg_right-emg_left); |
john111222333 | 20:c3be3bb3b515 | 42 | if (fabs(emg_right - emg_left) < 0.008 ) |
john111222333 | 16:a2a73d57d556 | 43 | { |
john111222333 | 16:a2a73d57d556 | 44 | return 0; |
john111222333 | 16:a2a73d57d556 | 45 | } |
john111222333 | 16:a2a73d57d556 | 46 | else |
john111222333 | 16:a2a73d57d556 | 47 | { |
john111222333 | 16:a2a73d57d556 | 48 | if ( emg_right - emg_left > 0) |
john111222333 | 16:a2a73d57d556 | 49 | return 0.1 ; |
john111222333 | 16:a2a73d57d556 | 50 | else |
john111222333 | 16:a2a73d57d556 | 51 | return -0.1 ; |
john111222333 | 16:a2a73d57d556 | 52 | } |
Alex_Kyrl | 9:22d79a4a0324 | 53 | } |
Alex_Kyrl | 8:0b7925095416 | 54 | |
Alex_Kyrl | 8:0b7925095416 | 55 | |
Alex_Kyrl | 9:22d79a4a0324 | 56 | double get_Y_control_signal(){ |
john111222333 | 16:a2a73d57d556 | 57 | double emg_right = EMG_tri_r.filter(); |
john111222333 | 16:a2a73d57d556 | 58 | double emg_left = EMG_tri_l.filter(); |
john111222333 | 16:a2a73d57d556 | 59 | //scope.set(1 ,emg_right-emg_left); |
john111222333 | 16:a2a73d57d556 | 60 | if (fabs(emg_right - emg_left) < 0.008 ) |
john111222333 | 16:a2a73d57d556 | 61 | { |
john111222333 | 16:a2a73d57d556 | 62 | return 0; |
john111222333 | 16:a2a73d57d556 | 63 | } |
john111222333 | 16:a2a73d57d556 | 64 | else |
john111222333 | 16:a2a73d57d556 | 65 | { |
john111222333 | 16:a2a73d57d556 | 66 | if ( emg_right - emg_left > 0) |
john111222333 | 16:a2a73d57d556 | 67 | return 0.1 ; |
john111222333 | 16:a2a73d57d556 | 68 | else |
john111222333 | 16:a2a73d57d556 | 69 | return -0.1 ; |
john111222333 | 16:a2a73d57d556 | 70 | } |
Alex_Kyrl | 8:0b7925095416 | 71 | } |
Alex_Kyrl | 8:0b7925095416 | 72 | |
john111222333 | 16:a2a73d57d556 | 73 | |
Alex_Kyrl | 8:0b7925095416 | 74 | /******************************************************/ |
Alex_Kyrl | 9:22d79a4a0324 | 75 | //set speed of setpoints |
Alex_Kyrl | 9:22d79a4a0324 | 76 | void control_motors() |
Alex_Kyrl | 9:22d79a4a0324 | 77 | { |
Alex_Kyrl | 8:0b7925095416 | 78 | |
john111222333 | 20:c3be3bb3b515 | 79 | float time_step = 0.1; //set the sample time |
john111222333 | 14:f561498eee28 | 80 | float threshold = 0.01; //set the threshold for cos(theta_2) |
john111222333 | 10:bb9a00d656c4 | 81 | float L1 = 0.48, L2 = 0.84; //set the lenght of arm 1 and 2 |
Alex_Kyrl | 13:559f8946f16d | 82 | float theta_1 = 2*3.14*motor1.set_angle()/360; |
Alex_Kyrl | 13:559f8946f16d | 83 | float theta_2 = 2*3.14*motor2.set_angle()/360; //get the angles |
john111222333 | 20:c3be3bb3b515 | 84 | float speed_X_axis = get_X_control_signal(); |
john111222333 | 17:fa80f1bc899b | 85 | float speed_Y_axis = 0;//get_Y_control_signal(); //get the desired velocitys |
Alex_Kyrl | 13:559f8946f16d | 86 | static float q_setpoint1 = 2*3.14*angle_start1/360; |
Alex_Kyrl | 13:559f8946f16d | 87 | static float q_setpoint2 = 2*3.14*angle_start2/360; //define the setpoint for motor 1 and 2 |
john111222333 | 14:f561498eee28 | 88 | |
Alex_Kyrl | 13:559f8946f16d | 89 | double x_poss = L2*sin(theta_1 + theta_2) + L1*cos(theta_1); |
Alex_Kyrl | 13:559f8946f16d | 90 | double y_poss = L1*sin(theta_1) - L2*cos(theta_1 + theta_2); |
Alex_Kyrl | 13:559f8946f16d | 91 | |
Alex_Kyrl | 13:559f8946f16d | 92 | double radius = sqrt(x_poss * x_poss + y_poss * y_poss) ; |
Alex_Kyrl | 13:559f8946f16d | 93 | |
john111222333 | 10:bb9a00d656c4 | 94 | |
john111222333 | 20:c3be3bb3b515 | 95 | if( fabs(q_setpoint1*360.0/(2*3.14) - theta_1*360/(2*3.14)) <= 1 and fabs(q_setpoint2*360.0/(2*3.14) -theta_2*360.0/(2*3.14)) <= 1 ) |
Alex_Kyrl | 13:559f8946f16d | 96 | { |
Alex_Kyrl | 13:559f8946f16d | 97 | |
Alex_Kyrl | 13:559f8946f16d | 98 | if( cos(theta_2) >= 0 and cos(theta_2) < threshold ) |
Alex_Kyrl | 13:559f8946f16d | 99 | { |
Alex_Kyrl | 13:559f8946f16d | 100 | q_setpoint1 = theta_1 + (time_step*(speed_Y_axis*cos(theta_1 + theta_2) - speed_X_axis*sin(theta_1 + theta_2)))/(L1*(threshold)); |
Alex_Kyrl | 13:559f8946f16d | 101 | q_setpoint2 = theta_2 + (time_step*(L1*speed_Y_axis*sin(theta_1) - L2*speed_Y_axis*cos(theta_1 + theta_2) + L2*speed_X_axis*sin(theta_1 + theta_2) + L1*speed_X_axis*cos(theta_1)))/(L1*L2*(threshold)); |
Alex_Kyrl | 13:559f8946f16d | 102 | } |
Alex_Kyrl | 13:559f8946f16d | 103 | else if( cos(theta_2) < 0 and cos(theta_2) > -threshold) |
Alex_Kyrl | 13:559f8946f16d | 104 | { |
Alex_Kyrl | 13:559f8946f16d | 105 | q_setpoint1 = theta_1 + (time_step*(speed_Y_axis*cos(theta_1 + theta_2) - speed_X_axis*sin(theta_1 + theta_2)))/(L1*(-threshold)); |
Alex_Kyrl | 13:559f8946f16d | 106 | q_setpoint2 = theta_2 + (time_step*(L1*speed_Y_axis*sin(theta_1) - L2*speed_Y_axis*cos(theta_1 + theta_2) + L2*speed_X_axis*sin(theta_1 + theta_2) + L1*speed_X_axis*cos(theta_1)))/(L1*L2*(-threshold)); |
Alex_Kyrl | 13:559f8946f16d | 107 | } |
Alex_Kyrl | 13:559f8946f16d | 108 | else |
Alex_Kyrl | 13:559f8946f16d | 109 | { |
Alex_Kyrl | 13:559f8946f16d | 110 | q_setpoint1 = theta_1 + (time_step*(speed_Y_axis*cos(theta_1 + theta_2) - speed_X_axis*sin(theta_1 + theta_2)))/(L1*cos(theta_2)); |
Alex_Kyrl | 13:559f8946f16d | 111 | q_setpoint2 = theta_2 + (time_step*(L1*speed_Y_axis*sin(theta_1) - L2*speed_Y_axis*cos(theta_1 + theta_2) + L2*speed_X_axis*sin(theta_1 + theta_2) + L1*speed_X_axis*cos(theta_1)))/(L1*L2*cos(theta_2)); |
Alex_Kyrl | 13:559f8946f16d | 112 | } |
john111222333 | 20:c3be3bb3b515 | 113 | if(q_setpoint1*360.0/(2*3.14)>100) |
john111222333 | 18:c5b408405e3d | 114 | { |
john111222333 | 20:c3be3bb3b515 | 115 | q_setpoint1 = (100.0/360)*2*3.14; |
john111222333 | 18:c5b408405e3d | 116 | } |
john111222333 | 18:c5b408405e3d | 117 | |
john111222333 | 20:c3be3bb3b515 | 118 | if(q_setpoint1*360.0/(2*3.14)<50) |
john111222333 | 18:c5b408405e3d | 119 | { |
john111222333 | 20:c3be3bb3b515 | 120 | q_setpoint1 = (50.0/360)*2*3.14; |
john111222333 | 18:c5b408405e3d | 121 | } |
john111222333 | 20:c3be3bb3b515 | 122 | if(q_setpoint2*360.0/(2*3.14)>140) |
john111222333 | 18:c5b408405e3d | 123 | { |
john111222333 | 20:c3be3bb3b515 | 124 | q_setpoint2 = (140.0/360)*2*3.14; |
john111222333 | 18:c5b408405e3d | 125 | } |
john111222333 | 18:c5b408405e3d | 126 | |
john111222333 | 18:c5b408405e3d | 127 | if(q_setpoint2*360/(2*3.14)<100) |
john111222333 | 18:c5b408405e3d | 128 | { |
john111222333 | 20:c3be3bb3b515 | 129 | q_setpoint2 = (100.0/360)*2*3.14; |
john111222333 | 18:c5b408405e3d | 130 | } |
john111222333 | 18:c5b408405e3d | 131 | |
john111222333 | 18:c5b408405e3d | 132 | |
john111222333 | 10:bb9a00d656c4 | 133 | } |
john111222333 | 10:bb9a00d656c4 | 134 | |
john111222333 | 10:bb9a00d656c4 | 135 | |
john111222333 | 16:a2a73d57d556 | 136 | scope.set(0, theta_1*360/(2*3.14)); |
john111222333 | 20:c3be3bb3b515 | 137 | scope.set(1, speed_X_axis);//theta_2*360/(2*3.14));// |
john111222333 | 16:a2a73d57d556 | 138 | scope.set(2, q_setpoint1*360/(2*3.14)); |
john111222333 | 16:a2a73d57d556 | 139 | scope.set(3, q_setpoint2*360/(2*3.14)); |
john111222333 | 14:f561498eee28 | 140 | if (!a){ |
john111222333 | 18:c5b408405e3d | 141 | scope.set(4, q_setpoint1*360/(2*3.14)-theta_1*360/(2*3.14));//motor1.Control_angle(5.0)); |
john111222333 | 18:c5b408405e3d | 142 | scope.set(5, q_setpoint2*360/(2*3.14)-theta_2*360/(2*3.14));//motor2.Control_angle(150.0)); |
john111222333 | 14:f561498eee28 | 143 | } |
john111222333 | 14:f561498eee28 | 144 | else{ |
john111222333 | 14:f561498eee28 | 145 | scope.set(4, motor1.Control_angle(q_setpoint1*360/(2*3.14))); |
john111222333 | 14:f561498eee28 | 146 | scope.set(5, motor2.Control_angle(q_setpoint2*360/(2*3.14))); |
john111222333 | 14:f561498eee28 | 147 | } |
john111222333 | 14:f561498eee28 | 148 | |
Alex_Kyrl | 9:22d79a4a0324 | 149 | } |
Alex_Kyrl | 9:22d79a4a0324 | 150 | |
Alex_Kyrl | 9:22d79a4a0324 | 151 | /******************************************************/ |
Alex_Kyrl | 8:0b7925095416 | 152 | // Ticker Function: |
Alex_Kyrl | 9:22d79a4a0324 | 153 | |
Alex_Kyrl | 9:22d79a4a0324 | 154 | void mainTicker() |
poephoofd | 0:2d9dae739559 | 155 | { |
Alex_Kyrl | 9:22d79a4a0324 | 156 | control_motors(); |
poephoofd | 0:2d9dae739559 | 157 | scope.send(); |
poephoofd | 2:a08bff88216d | 158 | } |
poephoofd | 2:a08bff88216d | 159 | |
Alex_Kyrl | 8:0b7925095416 | 160 | |
Alex_Kyrl | 8:0b7925095416 | 161 | /***************************************************/ |
Alex_Kyrl | 9:22d79a4a0324 | 162 | //Main Function: |
Alex_Kyrl | 8:0b7925095416 | 163 | |
poephoofd | 2:a08bff88216d | 164 | int main(void) |
poephoofd | 2:a08bff88216d | 165 | { |
Alex_Kyrl | 8:0b7925095416 | 166 | |
Alex_Kyrl | 9:22d79a4a0324 | 167 | double sample_time= 0.002; //fs = 500Hz |
Alex_Kyrl | 9:22d79a4a0324 | 168 | pc.baud(115200); //Set Baud rate for Serial communication |
Alex_Kyrl | 9:22d79a4a0324 | 169 | MainTicker.attach(&mainTicker, sample_time); //Attach time based interrupt |
poephoofd | 0:2d9dae739559 | 170 | |
Alex_Kyrl | 9:22d79a4a0324 | 171 | |
poephoofd | 2:a08bff88216d | 172 | while(true) |
poephoofd | 2:a08bff88216d | 173 | { |
Alex_Kyrl | 8:0b7925095416 | 174 | if(a==0){ |
Alex_Kyrl | 13:559f8946f16d | 175 | cont+=0.033; |
Alex_Kyrl | 8:0b7925095416 | 176 | wait(0.1); |
Alex_Kyrl | 8:0b7925095416 | 177 | } |
Alex_Kyrl | 8:0b7925095416 | 178 | if(b==0){ |
Alex_Kyrl | 13:559f8946f16d | 179 | cont-=0.033; |
Alex_Kyrl | 8:0b7925095416 | 180 | wait(0.1); |
Alex_Kyrl | 8:0b7925095416 | 181 | } |
poephoofd | 2:a08bff88216d | 182 | } |
Alex_Kyrl | 8:0b7925095416 | 183 | |
poephoofd | 2:a08bff88216d | 184 | |
Alex_Kyrl | 8:0b7925095416 | 185 | //return 0; |
poephoofd | 0:2d9dae739559 | 186 | } |