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@10:bb9a00d656c4, 2017-10-30 (annotated)

Committer:
john111222333
Date:
Mon Oct 30 15:24:49 2017 +0000
Revision:
10:bb9a00d656c4
Parent:
9:22d79a4a0324
Child:
12:69a9cf74583e
new inverse kinematics

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);
Alex_Kyrl 9:22d79a4a0324 8 double cont = 0 ;
Alex_Kyrl 9:22d79a4a0324 9
Alex_Kyrl 9:22d79a4a0324 10 HIDScope scope(4); // 4 channels of data
poephoofd 0:2d9dae739559 11 Ticker MainTicker;
poephoofd 0:2d9dae739559 12 MODSERIAL pc(USBTX, USBRX);
poephoofd 0:2d9dae739559 13
Alex_Kyrl 8:0b7925095416 14 /*****************************************************************/
Alex_Kyrl 8:0b7925095416 15 //Initialize Analog EMG inputs:
Alex_Kyrl 8:0b7925095416 16
Alex_Kyrl 9:22d79a4a0324 17
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:
Alex_Kyrl 8:0b7925095416 25
Alex_Kyrl 9:22d79a4a0324 26 Motor motor2(D13 , D12 , D7 , D6 , 50000 , 180 , 0.6 );
Alex_Kyrl 9:22d79a4a0324 27 Motor motor1(D11 , D10 , D4 , D5 , 50000 , 180 , 0.2 );
poephoofd 0:2d9dae739559 28
Alex_Kyrl 8:0b7925095416 29 /*****************************************************/
Alex_Kyrl 8:0b7925095416 30 // Set control signals:
poephoofd 0:2d9dae739559 31
Alex_Kyrl 9:22d79a4a0324 32 //x direction is the righ/left movement
Alex_Kyrl 9:22d79a4a0324 33 //y direction is forward/backward movement
Alex_Kyrl 9:22d79a4a0324 34
Alex_Kyrl 9:22d79a4a0324 35 double get_X_control_signal(){
Alex_Kyrl 9:22d79a4a0324 36 double emg_right = EMG_bi_r.filter();
Alex_Kyrl 9:22d79a4a0324 37 double emg_left = EMG_bi_l.filter();
Alex_Kyrl 9:22d79a4a0324 38 // TODO: Tune emg to velocity mapping
Alex_Kyrl 9:22d79a4a0324 39 return emg_right - emg_left;
Alex_Kyrl 9:22d79a4a0324 40
Alex_Kyrl 9:22d79a4a0324 41 }
Alex_Kyrl 8:0b7925095416 42
Alex_Kyrl 8:0b7925095416 43
Alex_Kyrl 9:22d79a4a0324 44 double get_Y_control_signal(){
Alex_Kyrl 9:22d79a4a0324 45 double emg_fwd= EMG_tri_r.filter();
Alex_Kyrl 9:22d79a4a0324 46 double emg_bwd= EMG_tri_l.filter();
Alex_Kyrl 9:22d79a4a0324 47 // TODO: `Tune emg to velocity mapping
Alex_Kyrl 9:22d79a4a0324 48 return cont;//emg_fwd - emg_bwd;
Alex_Kyrl 8:0b7925095416 49
Alex_Kyrl 8:0b7925095416 50 }
Alex_Kyrl 8:0b7925095416 51
Alex_Kyrl 8:0b7925095416 52 /******************************************************/
Alex_Kyrl 9:22d79a4a0324 53 //set speed of setpoints
Alex_Kyrl 9:22d79a4a0324 54 void control_motors()
Alex_Kyrl 9:22d79a4a0324 55 {
john111222333 10:bb9a00d656c4 56 /*
Alex_Kyrl 9:22d79a4a0324 57 int row_J =2 , row_Speed=2 , column_J =2;
Alex_Kyrl 9:22d79a4a0324 58 float speed_setpoint[row_J] , J_inv[row_J][column_J] , speed[row_Speed];
Alex_Kyrl 9:22d79a4a0324 59
Alex_Kyrl 9:22d79a4a0324 60 speed[0] = 0;//get_X_control_signal();
Alex_Kyrl 9:22d79a4a0324 61 speed[1] = get_Y_control_signal();
Alex_Kyrl 9:22d79a4a0324 62
Alex_Kyrl 9:22d79a4a0324 63 float theta_1 = 2*3.14*motor1.set_angle()/360 , theta_2 = 2*3.14*motor2.set_angle()/360;
Alex_Kyrl 9:22d79a4a0324 64 float L1 = 0.48;
Alex_Kyrl 9:22d79a4a0324 65 float L2 = 0.84;
Alex_Kyrl 9:22d79a4a0324 66
Alex_Kyrl 9:22d79a4a0324 67 J_inv[0][0] = -sin(theta_1 + theta_2)/(L1*cos(theta_2)) ;
Alex_Kyrl 9:22d79a4a0324 68 J_inv[0][1] = cos(theta_1 + theta_2)/(L1*cos(theta_2)) ;
Alex_Kyrl 9:22d79a4a0324 69 J_inv[1][0] = (L2*sin(theta_1 + theta_2) + L1*cos(theta_1))/(L1*L2*cos(theta_2)) ;
Alex_Kyrl 9:22d79a4a0324 70 J_inv[1][1] = -(L2*cos(theta_1 + theta_2) - L1*sin(theta_1))/(L1*L2*cos(theta_2)) ;
Alex_Kyrl 9:22d79a4a0324 71
Alex_Kyrl 9:22d79a4a0324 72 // Initializing elements of matrix mult to 0.
Alex_Kyrl 9:22d79a4a0324 73 for(int i = 0; i < row_J; ++i)
Alex_Kyrl 9:22d79a4a0324 74 {
Alex_Kyrl 9:22d79a4a0324 75 speed_setpoint[i] = 0;
john111222333 10:bb9a00d656c4 76
john111222333 10:bb9a00d656c4 77
john111222333 10:bb9a00d656c4 78
john111222333 10:bb9a00d656c4 79
Alex_Kyrl 9:22d79a4a0324 80 }
Alex_Kyrl 8:0b7925095416 81
Alex_Kyrl 9:22d79a4a0324 82 // Multiplying matrix firstMatrix and secondMatrix and storing in array mult.
Alex_Kyrl 9:22d79a4a0324 83 for(int i = 0; i < row_J; ++i)
Alex_Kyrl 9:22d79a4a0324 84 {
Alex_Kyrl 9:22d79a4a0324 85 for(int k=0; k<column_J; ++k)
Alex_Kyrl 9:22d79a4a0324 86 {
Alex_Kyrl 9:22d79a4a0324 87 speed_setpoint[i] += J_inv[i][k] * speed[k];
Alex_Kyrl 9:22d79a4a0324 88 }
Alex_Kyrl 9:22d79a4a0324 89 }
john111222333 10:bb9a00d656c4 90 float time = 0.002 ;
john111222333 10:bb9a00d656c4 91 motor1.Control_angle(theta_1*360/(2*3.14) + speed_setpoint[0]*time*360/(2*3.14));
john111222333 10:bb9a00d656c4 92 motor2.Control_angle(theta_2*360/(2*3.14) + speed_setpoint[1]*time*360/(2*3.14));
john111222333 10:bb9a00d656c4 93 */
john111222333 10:bb9a00d656c4 94
john111222333 10:bb9a00d656c4 95
john111222333 10:bb9a00d656c4 96 float T = 0.002; //set the sample time
john111222333 10:bb9a00d656c4 97 float threshold = 0.01; //set the threshold for cos(theta_2)
john111222333 10:bb9a00d656c4 98 float L1 = 0.48, L2 = 0.84; //set the lenght of arm 1 and 2
john111222333 10:bb9a00d656c4 99 float theta_1 = 2*3.14*motor1.set_angle()/360 , theta_2 = 2*3.14*motor2.set_angle()/360; //get the angles
john111222333 10:bb9a00d656c4 100 float v_x = get_X_control_signal(), v_y = get_Y_control_signal(); //get the desired velocitys
john111222333 10:bb9a00d656c4 101 float q_setpoint1, q_setpoint2; //define the setpoint for motor 1 and 2
john111222333 10:bb9a00d656c4 102
john111222333 10:bb9a00d656c4 103 if((cos(theta_2))>=0 && cos(theta_2)<threshold){
john111222333 10:bb9a00d656c4 104
john111222333 10:bb9a00d656c4 105 q_setpoint1=theta_1 + (T*(v_y*cos(theta_1 + theta_2) - v_x*sin(theta_1 + theta_2)))/(L1*(threshold));
john111222333 10:bb9a00d656c4 106 q_setpoint2=theta_2 + (T*(L1*v_y*sin(theta_1) - L2*v_y*cos(theta_1 + theta_2) + L2*v_x*sin(theta_1 + theta_2) + L1*v_x*cos(theta_1)))/(L1*L2*(threshold));
john111222333 10:bb9a00d656c4 107
john111222333 10:bb9a00d656c4 108 }
john111222333 10:bb9a00d656c4 109 else if((cos(theta_2))<0 && cos(theta_2)>-threshold){
john111222333 10:bb9a00d656c4 110
john111222333 10:bb9a00d656c4 111 q_setpoint1=theta_1 + (T*(v_y*cos(theta_1 + theta_2) - v_x*sin(theta_1 + theta_2)))/(L1*(-threshold));
john111222333 10:bb9a00d656c4 112 q_setpoint2=theta_2 + (T*(L1*v_y*sin(theta_1) - L2*v_y*cos(theta_1 + theta_2) + L2*v_x*sin(theta_1 + theta_2) + L1*v_x*cos(theta_1)))/(L1*L2*(-threshold));
john111222333 10:bb9a00d656c4 113 }
john111222333 10:bb9a00d656c4 114 else{
john111222333 10:bb9a00d656c4 115 q_setpoint1=theta_1 + (T*(v_y*cos(theta_1 + theta_2) - v_x*sin(theta_1 + theta_2)))/(L1*cos(theta_2));
john111222333 10:bb9a00d656c4 116 q_setpoint2=theta_2 + (T*(L1*v_y*sin(theta_1) - L2*v_y*cos(theta_1 + theta_2) + L2*v_x*sin(theta_1 + theta_2) + L1*v_x*cos(theta_1)))/(L1*L2*cos(theta_2));
john111222333 10:bb9a00d656c4 117 }
john111222333 10:bb9a00d656c4 118
john111222333 10:bb9a00d656c4 119
Alex_Kyrl 9:22d79a4a0324 120
Alex_Kyrl 9:22d79a4a0324 121 scope.set(0, theta_1*360/(2*3.14));
Alex_Kyrl 9:22d79a4a0324 122 scope.set(1, cont);
john111222333 10:bb9a00d656c4 123 scope.set(2, v_x);
john111222333 10:bb9a00d656c4 124 scope.set(3, v_y);
john111222333 10:bb9a00d656c4 125 motor1.Control_angle(q_setpoint1*360/(2*3.14));
john111222333 10:bb9a00d656c4 126 motor2.Control_angle(q_setpoint2*360/(2*3.14));
Alex_Kyrl 9:22d79a4a0324 127 }
Alex_Kyrl 9:22d79a4a0324 128
Alex_Kyrl 9:22d79a4a0324 129 /******************************************************/
Alex_Kyrl 8:0b7925095416 130 // Ticker Function:
Alex_Kyrl 9:22d79a4a0324 131
Alex_Kyrl 9:22d79a4a0324 132 void mainTicker()
poephoofd 0:2d9dae739559 133 {
Alex_Kyrl 9:22d79a4a0324 134
Alex_Kyrl 9:22d79a4a0324 135
Alex_Kyrl 9:22d79a4a0324 136 control_motors();
poephoofd 0:2d9dae739559 137
Alex_Kyrl 9:22d79a4a0324 138
Alex_Kyrl 9:22d79a4a0324 139
Alex_Kyrl 9:22d79a4a0324 140
Alex_Kyrl 9:22d79a4a0324 141
Alex_Kyrl 9:22d79a4a0324 142 // scope.set(0, x_control_signal);
Alex_Kyrl 9:22d79a4a0324 143 // scope.set(1, motor2.set_angle());
poephoofd 0:2d9dae739559 144 scope.send();
Alex_Kyrl 8:0b7925095416 145
poephoofd 2:a08bff88216d 146 }
poephoofd 2:a08bff88216d 147
Alex_Kyrl 8:0b7925095416 148
Alex_Kyrl 8:0b7925095416 149 /***************************************************/
Alex_Kyrl 9:22d79a4a0324 150 //Main Function:
Alex_Kyrl 8:0b7925095416 151
poephoofd 2:a08bff88216d 152 int main(void)
poephoofd 2:a08bff88216d 153 {
Alex_Kyrl 8:0b7925095416 154
Alex_Kyrl 9:22d79a4a0324 155 double sample_time= 0.002; //fs = 500Hz
Alex_Kyrl 9:22d79a4a0324 156 pc.baud(115200); //Set Baud rate for Serial communication
Alex_Kyrl 9:22d79a4a0324 157 MainTicker.attach(&mainTicker, sample_time); //Attach time based interrupt
poephoofd 0:2d9dae739559 158
Alex_Kyrl 9:22d79a4a0324 159
poephoofd 2:a08bff88216d 160 while(true)
poephoofd 2:a08bff88216d 161 {
Alex_Kyrl 8:0b7925095416 162 if(a==0){
Alex_Kyrl 9:22d79a4a0324 163 cont+=0.1;
Alex_Kyrl 8:0b7925095416 164 wait(0.1);
Alex_Kyrl 8:0b7925095416 165 }
Alex_Kyrl 8:0b7925095416 166 if(b==0){
Alex_Kyrl 9:22d79a4a0324 167 cont-=0.1;
Alex_Kyrl 8:0b7925095416 168 wait(0.1);
Alex_Kyrl 8:0b7925095416 169 }
poephoofd 2:a08bff88216d 170 }
Alex_Kyrl 8:0b7925095416 171
poephoofd 2:a08bff88216d 172
Alex_Kyrl 8:0b7925095416 173 //return 0;
poephoofd 0:2d9dae739559 174 }