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@21:59431788a42d, 2017-11-06 (annotated)

Committer:
john111222333
Date:
Mon Nov 06 09:57:21 2017 +0000
Revision:
21:59431788a42d
Parent:
20:c3be3bb3b515 
final version;

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 ;
john111222333 21:59431788a42d 11 bool test;
Alex_Kyrl 9:22d79a4a0324 12
Alex_Kyrl 11:dd1976534a03 13 HIDScope scope(6); // 4 channels of data
poephoofd 0:2d9dae739559 14 Ticker MainTicker;
poephoofd 0:2d9dae739559 15 MODSERIAL pc(USBTX, USBRX);
poephoofd 0:2d9dae739559 16
Alex_Kyrl 8:0b7925095416 17 /*****************************************************************/
john111222333 14:f561498eee28 18 //Initialize Analog EMG inputs:
Alex_Kyrl 9:22d79a4a0324 19 EMG EMG_bi_r(A0); // Move the endpoint to the right (plus direction)
Alex_Kyrl 9:22d79a4a0324 20 EMG EMG_bi_l(A1); // Move the endpoint to the left (minus direction)
Alex_Kyrl 9:22d79a4a0324 21 EMG EMG_tri_r(A2); // Move the endpoint forward (plus direction)
Alex_Kyrl 9:22d79a4a0324 22 EMG EMG_tri_l(A3); // Move the endpoint backward (minus direction)
Alex_Kyrl 8:0b7925095416 23
Alex_Kyrl 8:0b7925095416 24 /****************************************************/
Alex_Kyrl 9:22d79a4a0324 25 //Initialise Motors:
john111222333 21:59431788a42d 26 int angle_start1 = 90;//51.3676;
john111222333 21:59431788a42d 27 int angle_start2 = 90;//140.2431;
Alex_Kyrl 8:0b7925095416 28
john111222333 21:59431788a42d 29 Motor motor1(D13 , D12 , D7 , D6 , 50000 , 180 , 0.7 , 0 , angle_start1 , 5 , 1, 0.00000);
john111222333 21:59431788a42d 30 Motor motor2(D11 , D10 , D4 , D5 , 50000 , 90 , 0.7 , 0 , angle_start2 , 7 , 1 , 0.00000 );
john111222333 16:a2a73d57d556 31
poephoofd 0:2d9dae739559 32
Alex_Kyrl 8:0b7925095416 33 /*****************************************************/
Alex_Kyrl 8:0b7925095416 34 // Set control signals:
poephoofd 0:2d9dae739559 35
Alex_Kyrl 9:22d79a4a0324 36 //x direction is the righ/left movement
Alex_Kyrl 9:22d79a4a0324 37 //y direction is forward/backward movement
Alex_Kyrl 9:22d79a4a0324 38
Alex_Kyrl 9:22d79a4a0324 39 double get_X_control_signal(){
Alex_Kyrl 9:22d79a4a0324 40 double emg_right = EMG_bi_r.filter();
john111222333 21:59431788a42d 41 double emg_left = 1.5*EMG_bi_l.filter();
john111222333 20:c3be3bb3b515 42 //scope.set(0 ,emg_right-emg_left);
john111222333 21:59431788a42d 43 if (fabs(emg_right - emg_left) < 0.002 )
john111222333 16:a2a73d57d556 44 {
john111222333 16:a2a73d57d556 45 return 0;
john111222333 16:a2a73d57d556 46 }
john111222333 16:a2a73d57d556 47 else
john111222333 16:a2a73d57d556 48 {
john111222333 16:a2a73d57d556 49 if ( emg_right - emg_left > 0)
john111222333 16:a2a73d57d556 50 return 0.1 ;
john111222333 16:a2a73d57d556 51 else
john111222333 16:a2a73d57d556 52 return -0.1 ;
john111222333 16:a2a73d57d556 53 }
Alex_Kyrl 9:22d79a4a0324 54 }
Alex_Kyrl 8:0b7925095416 55
Alex_Kyrl 8:0b7925095416 56
Alex_Kyrl 9:22d79a4a0324 57 double get_Y_control_signal(){
john111222333 16:a2a73d57d556 58 double emg_right = EMG_tri_r.filter();
john111222333 21:59431788a42d 59 double emg_left = 1.5*EMG_tri_l.filter();
john111222333 16:a2a73d57d556 60 //scope.set(1 ,emg_right-emg_left);
john111222333 21:59431788a42d 61 if (fabs(emg_right - emg_left) < 0.002 )
john111222333 16:a2a73d57d556 62 {
john111222333 16:a2a73d57d556 63 return 0;
john111222333 16:a2a73d57d556 64 }
john111222333 16:a2a73d57d556 65 else
john111222333 16:a2a73d57d556 66 {
john111222333 16:a2a73d57d556 67 if ( emg_right - emg_left > 0)
john111222333 16:a2a73d57d556 68 return 0.1 ;
john111222333 16:a2a73d57d556 69 else
john111222333 16:a2a73d57d556 70 return -0.1 ;
john111222333 16:a2a73d57d556 71 }
Alex_Kyrl 8:0b7925095416 72 }
Alex_Kyrl 8:0b7925095416 73
john111222333 16:a2a73d57d556 74
Alex_Kyrl 8:0b7925095416 75 /******************************************************/
Alex_Kyrl 9:22d79a4a0324 76 //set speed of setpoints
Alex_Kyrl 9:22d79a4a0324 77 void control_motors()
Alex_Kyrl 9:22d79a4a0324 78 {
Alex_Kyrl 8:0b7925095416 79
john111222333 21:59431788a42d 80
Alex_Kyrl 13:559f8946f16d 81 float theta_1 = 2*3.14*motor1.set_angle()/360;
Alex_Kyrl 13:559f8946f16d 82 float theta_2 = 2*3.14*motor2.set_angle()/360; //get the angles
john111222333 21:59431788a42d 83 float speed_X_axis = 0 , speed_Y_axis = 0 ;
john111222333 21:59431788a42d 84 if (test)
john111222333 21:59431788a42d 85 speed_X_axis = get_X_control_signal();
john111222333 21:59431788a42d 86 else
john111222333 21:59431788a42d 87 speed_Y_axis = get_X_control_signal(); //get the desired velocitys
Alex_Kyrl 13:559f8946f16d 88
john111222333 10:bb9a00d656c4 89
john111222333 10:bb9a00d656c4 90
john111222333 10:bb9a00d656c4 91
john111222333 16:a2a73d57d556 92 scope.set(0, theta_1*360/(2*3.14));
john111222333 21:59431788a42d 93 scope.set(1, theta_2*360/(2*3.14));//
john111222333 21:59431788a42d 94 scope.set(2, speed_X_axis);
john111222333 21:59431788a42d 95 scope.set(3, speed_Y_axis);
john111222333 21:59431788a42d 96 scope.set(4, motor1.Control_angle(theta_1*360.0/(2*3.14) + speed_X_axis*5));
john111222333 21:59431788a42d 97 scope.set(5, motor2.Control_angle(theta_2*360.0/(2*3.14) + speed_Y_axis*5));
john111222333 21:59431788a42d 98
john111222333 14:f561498eee28 99
Alex_Kyrl 9:22d79a4a0324 100 }
Alex_Kyrl 9:22d79a4a0324 101
Alex_Kyrl 9:22d79a4a0324 102 /******************************************************/
Alex_Kyrl 8:0b7925095416 103 // Ticker Function:
Alex_Kyrl 9:22d79a4a0324 104
Alex_Kyrl 9:22d79a4a0324 105 void mainTicker()
poephoofd 0:2d9dae739559 106 {
Alex_Kyrl 9:22d79a4a0324 107 control_motors();
poephoofd 0:2d9dae739559 108 scope.send();
poephoofd 2:a08bff88216d 109 }
poephoofd 2:a08bff88216d 110
Alex_Kyrl 8:0b7925095416 111
Alex_Kyrl 8:0b7925095416 112 /***************************************************/
Alex_Kyrl 9:22d79a4a0324 113 //Main Function:
Alex_Kyrl 8:0b7925095416 114
poephoofd 2:a08bff88216d 115 int main(void)
poephoofd 2:a08bff88216d 116 {
Alex_Kyrl 8:0b7925095416 117
Alex_Kyrl 9:22d79a4a0324 118 double sample_time= 0.002; //fs = 500Hz
Alex_Kyrl 9:22d79a4a0324 119 pc.baud(115200); //Set Baud rate for Serial communication
Alex_Kyrl 9:22d79a4a0324 120 MainTicker.attach(&mainTicker, sample_time); //Attach time based interrupt
poephoofd 0:2d9dae739559 121
Alex_Kyrl 9:22d79a4a0324 122
poephoofd 2:a08bff88216d 123 while(true)
poephoofd 2:a08bff88216d 124 {
Alex_Kyrl 8:0b7925095416 125 if(a==0){
john111222333 21:59431788a42d 126 test=!test;
john111222333 21:59431788a42d 127 wait(0.5);
Alex_Kyrl 8:0b7925095416 128 }
Alex_Kyrl 8:0b7925095416 129 if(b==0){
Alex_Kyrl 13:559f8946f16d 130 cont-=0.033;
Alex_Kyrl 8:0b7925095416 131 wait(0.1);
Alex_Kyrl 8:0b7925095416 132 }
poephoofd 2:a08bff88216d 133 }
Alex_Kyrl 8:0b7925095416 134
poephoofd 2:a08bff88216d 135
Alex_Kyrl 8:0b7925095416 136 //return 0;
poephoofd 0:2d9dae739559 137 }