Poep Hoofd / Mbed 2 deprecated PoolRobot_Code

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?

UserRevisionLine numberNew 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 }