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@13:559f8946f16d, 2017-11-01 (annotated)

Committer:
Alex_Kyrl
Date:
Wed Nov 01 07:11:00 2017 +0000
Revision:
13:559f8946f16d
Parent:
12:69a9cf74583e
Child:
14:f561498eee28
test why it doesn't work

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 11:dd1976534a03 10 HIDScope scope(6); // 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 13:559f8946f16d 25 int angle_start1 = 80;
Alex_Kyrl 13:559f8946f16d 26 int angle_start2 = 100;
Alex_Kyrl 8:0b7925095416 27
Alex_Kyrl 13:559f8946f16d 28 Motor motor1(D13 , D12 , D7 , D6 , 50000 , 90 , 0.5 , 0 , angle_start1);
Alex_Kyrl 13:559f8946f16d 29 Motor motor2(D11 , D10 , D4 , D5 , 50000 , 50 , 0.5 , 0 , angle_start2);
poephoofd 0:2d9dae739559 30
Alex_Kyrl 8:0b7925095416 31 /*****************************************************/
Alex_Kyrl 8:0b7925095416 32 // Set control signals:
poephoofd 0:2d9dae739559 33
Alex_Kyrl 9:22d79a4a0324 34 //x direction is the righ/left movement
Alex_Kyrl 9:22d79a4a0324 35 //y direction is forward/backward movement
Alex_Kyrl 9:22d79a4a0324 36
Alex_Kyrl 9:22d79a4a0324 37 double get_X_control_signal(){
Alex_Kyrl 9:22d79a4a0324 38 double emg_right = EMG_bi_r.filter();
Alex_Kyrl 9:22d79a4a0324 39 double emg_left = EMG_bi_l.filter();
Alex_Kyrl 13:559f8946f16d 40 // TODO: time_stepune emg to velocity mapping
Alex_Kyrl 13:559f8946f16d 41 return 0;// emg_right - emg_left;
Alex_Kyrl 9:22d79a4a0324 42
Alex_Kyrl 9:22d79a4a0324 43 }
Alex_Kyrl 8:0b7925095416 44
Alex_Kyrl 8:0b7925095416 45
Alex_Kyrl 9:22d79a4a0324 46 double get_Y_control_signal(){
Alex_Kyrl 9:22d79a4a0324 47 double emg_fwd= EMG_tri_r.filter();
Alex_Kyrl 9:22d79a4a0324 48 double emg_bwd= EMG_tri_l.filter();
Alex_Kyrl 13:559f8946f16d 49 // TODO: `time_stepune emg to velocity mapping
Alex_Kyrl 11:dd1976534a03 50 return cont;// emg_fwd - emg_bwd;
Alex_Kyrl 8:0b7925095416 51
Alex_Kyrl 8:0b7925095416 52 }
Alex_Kyrl 8:0b7925095416 53
Alex_Kyrl 8:0b7925095416 54 /******************************************************/
Alex_Kyrl 9:22d79a4a0324 55 //set speed of setpoints
Alex_Kyrl 9:22d79a4a0324 56 void control_motors()
Alex_Kyrl 9:22d79a4a0324 57 {
Alex_Kyrl 8:0b7925095416 58
Alex_Kyrl 12:69a9cf74583e 59 float time_step = 0.002; //set the sample time
Alex_Kyrl 13:559f8946f16d 60 float threshold = 0.05; //set the threshold for cos(theta_2)
john111222333 10:bb9a00d656c4 61 float L1 = 0.48, L2 = 0.84; //set the lenght of arm 1 and 2
Alex_Kyrl 13:559f8946f16d 62 float theta_1 = 2*3.14*motor1.set_angle()/360;
Alex_Kyrl 13:559f8946f16d 63 float theta_2 = 2*3.14*motor2.set_angle()/360; //get the angles
Alex_Kyrl 13:559f8946f16d 64 float speed_X_axis = get_X_control_signal();
Alex_Kyrl 12:69a9cf74583e 65 float speed_Y_axis = get_Y_control_signal(); //get the desired velocitys
Alex_Kyrl 13:559f8946f16d 66 static float q_setpoint1 = 2*3.14*angle_start1/360;
Alex_Kyrl 13:559f8946f16d 67 static float q_setpoint2 = 2*3.14*angle_start2/360; //define the setpoint for motor 1 and 2
Alex_Kyrl 13:559f8946f16d 68
Alex_Kyrl 13:559f8946f16d 69 double x_poss = L2*sin(theta_1 + theta_2) + L1*cos(theta_1);
Alex_Kyrl 13:559f8946f16d 70 double y_poss = L1*sin(theta_1) - L2*cos(theta_1 + theta_2);
Alex_Kyrl 13:559f8946f16d 71
Alex_Kyrl 13:559f8946f16d 72 double radius = sqrt(x_poss * x_poss + y_poss * y_poss) ;
Alex_Kyrl 13:559f8946f16d 73
john111222333 10:bb9a00d656c4 74
Alex_Kyrl 13:559f8946f16d 75 if( radius < (L1 + L2) )
Alex_Kyrl 13:559f8946f16d 76 {
Alex_Kyrl 13:559f8946f16d 77
Alex_Kyrl 13:559f8946f16d 78 if( cos(theta_2) >= 0 and cos(theta_2) < threshold )
Alex_Kyrl 13:559f8946f16d 79 {
Alex_Kyrl 13:559f8946f16d 80 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 81 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 82 }
Alex_Kyrl 13:559f8946f16d 83 else if( cos(theta_2) < 0 and cos(theta_2) > -threshold)
Alex_Kyrl 13:559f8946f16d 84 {
Alex_Kyrl 13:559f8946f16d 85 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 86 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 87 }
Alex_Kyrl 13:559f8946f16d 88 else
Alex_Kyrl 13:559f8946f16d 89 {
Alex_Kyrl 13:559f8946f16d 90 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 91 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 92 }
john111222333 10:bb9a00d656c4 93 }
john111222333 10:bb9a00d656c4 94
john111222333 10:bb9a00d656c4 95
Alex_Kyrl 9:22d79a4a0324 96 scope.set(0, theta_1*360/(2*3.14));
Alex_Kyrl 13:559f8946f16d 97 scope.set(1, theta_2*360/(2*3.14));
Alex_Kyrl 13:559f8946f16d 98 scope.set(2, cont);
Alex_Kyrl 13:559f8946f16d 99 scope.set(3, radius);
Alex_Kyrl 12:69a9cf74583e 100 scope.set(4, motor1.Control_angle(q_setpoint1*360/(2*3.14)) );
Alex_Kyrl 13:559f8946f16d 101 scope.set(5, motor2.Control_angle(q_setpoint2*360/(2*3.14)) );
Alex_Kyrl 11:dd1976534a03 102
Alex_Kyrl 9:22d79a4a0324 103 }
Alex_Kyrl 9:22d79a4a0324 104
Alex_Kyrl 9:22d79a4a0324 105 /******************************************************/
Alex_Kyrl 8:0b7925095416 106 // Ticker Function:
Alex_Kyrl 9:22d79a4a0324 107
Alex_Kyrl 9:22d79a4a0324 108 void mainTicker()
poephoofd 0:2d9dae739559 109 {
Alex_Kyrl 9:22d79a4a0324 110 control_motors();
poephoofd 0:2d9dae739559 111 scope.send();
poephoofd 2:a08bff88216d 112 }
poephoofd 2:a08bff88216d 113
Alex_Kyrl 8:0b7925095416 114
Alex_Kyrl 8:0b7925095416 115 /***************************************************/
Alex_Kyrl 9:22d79a4a0324 116 //Main Function:
Alex_Kyrl 8:0b7925095416 117
poephoofd 2:a08bff88216d 118 int main(void)
poephoofd 2:a08bff88216d 119 {
Alex_Kyrl 8:0b7925095416 120
Alex_Kyrl 9:22d79a4a0324 121 double sample_time= 0.002; //fs = 500Hz
Alex_Kyrl 9:22d79a4a0324 122 pc.baud(115200); //Set Baud rate for Serial communication
Alex_Kyrl 9:22d79a4a0324 123 MainTicker.attach(&mainTicker, sample_time); //Attach time based interrupt
poephoofd 0:2d9dae739559 124
Alex_Kyrl 9:22d79a4a0324 125
poephoofd 2:a08bff88216d 126 while(true)
poephoofd 2:a08bff88216d 127 {
Alex_Kyrl 8:0b7925095416 128 if(a==0){
Alex_Kyrl 13:559f8946f16d 129 cont+=0.033;
Alex_Kyrl 8:0b7925095416 130 wait(0.1);
Alex_Kyrl 8:0b7925095416 131 }
Alex_Kyrl 8:0b7925095416 132 if(b==0){
Alex_Kyrl 13:559f8946f16d 133 cont-=0.033;
Alex_Kyrl 8:0b7925095416 134 wait(0.1);
Alex_Kyrl 8:0b7925095416 135 }
poephoofd 2:a08bff88216d 136 }
Alex_Kyrl 8:0b7925095416 137
poephoofd 2:a08bff88216d 138
Alex_Kyrl 8:0b7925095416 139 //return 0;
poephoofd 0:2d9dae739559 140 }