Luuk Lomillos Rozeboom / Mbed 2 deprecated RobotControlFinal

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 MODSERIAL QEI biquadFilter mbed

main.cpp@4:2786719c73fd, 2017-10-26 (annotated)

Committer:
Luuk
Date:
Thu Oct 26 22:02:24 2017 +0000
Revision:
4:2786719c73fd
Parent:
3:03db694efdbe
Child:
5:77ffa336d6eb
with emg input

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Luuk 0:1b8a08e9a66c 1 #include "mbed.h"
Luuk 1:14b685c3abbd 2 #include "QEI.h"
Luuk 4:2786719c73fd 3 #include "BiQuad.h"
Luuk 0:1b8a08e9a66c 4 #include "MODSERIAL.h"
Luuk 0:1b8a08e9a66c 5 //#include "HIDScope.h"
Luuk 0:1b8a08e9a66c 6
Luuk 0:1b8a08e9a66c 7 Serial pc (USBTX,USBRX);
Luuk 1:14b685c3abbd 8
Luuk 1:14b685c3abbd 9 PwmOut Motor1Vel(D6); //motor 1 velocity control
Luuk 1:14b685c3abbd 10 DigitalOut Motor1Dir(D7); //motor 1 direction
Luuk 1:14b685c3abbd 11 PwmOut Motor2Vel(D5); //motor 2 velocity control
Luuk 1:14b685c3abbd 12 DigitalOut Motor2Dir(D4); //motor 2 direction
Luuk 1:14b685c3abbd 13 DigitalOut led1(D2);
Luuk 2:e36213affbda 14 AnalogIn emg1(A0);
Luuk 2:e36213affbda 15 AnalogIn emg2(A1);
Luuk 4:2786719c73fd 16 AnalogIn emg3(A3);
Luuk 2:e36213affbda 17 DigitalOut Button(D3);
Luuk 0:1b8a08e9a66c 18 //DigitalOut ledb(LED1);
Luuk 0:1b8a08e9a66c 19
Luuk 4:2786719c73fd 20 QEI encoder1(D8,D9,NC,16); //define encoder pins
Luuk 1:14b685c3abbd 21 QEI encoder2(D10,D11,NC,16);
Luuk 1:14b685c3abbd 22
Luuk 4:2786719c73fd 23 BiQuad bq1 ( 0.9150 , -1.8299 , 0.9150 , 1.0000 , -1.8227 , 0.8372); //Highpass
Luuk 4:2786719c73fd 24 BiQuad bq2 ( 0.0001 , 0.0002 , 0.0001 , 1.0000 , -1.9733 , 0.9737); //Lowpass
Luuk 4:2786719c73fd 25
Luuk 4:2786719c73fd 26 Ticker motors_tick;
Luuk 4:2786719c73fd 27 Ticker emg_tick;
Luuk 4:2786719c73fd 28
Luuk 3:03db694efdbe 29 float Angle1,Angle2; //real angles of the motor
Luuk 3:03db694efdbe 30 float XPos, YPos; //position of the end-effector
Luuk 3:03db694efdbe 31 float XSet, YSet; //desired position of the end-effector
Luuk 3:03db694efdbe 32 float L = 300.0; //length of the arms of the robot
Luuk 3:03db694efdbe 33 const float Ts = 0.01; //to control the tickers
Luuk 3:03db694efdbe 34 const float KV = 1; //velocity constant, to scale the desired velocity of the end-effector
Luuk 3:03db694efdbe 35
Luuk 4:2786719c73fd 36 volatile double y=0;
Luuk 4:2786719c73fd 37 const float pi = 3.14159265359,PulsesPerRev = 16.0,GearRatio = 131.15*3; //to convert pulses to radians
Luuk 3:03db694efdbe 38 const float Angle1_0 = pi/2 ,Angle2_0 = 0; //initial angle of the motors
Luuk 3:03db694efdbe 39
Luuk 3:03db694efdbe 40 //variables and constants for the PID
Luuk 3:03db694efdbe 41 const float KP = 10, KI = 5, KD = 2, N = 100;
Luuk 3:03db694efdbe 42 float M1_v1 = 0, M1_v2 = 0, M2_v1 = 0, M2_v2 = 0;
Luuk 3:03db694efdbe 43
Luuk 4:2786719c73fd 44 volatile double a = 0, b = 0, c = 0; //variables to store the highest emg signal to normalize the data
Luuk 4:2786719c73fd 45 double Y1, Y2; // Y1 and Y2 are taken to conform Y to be used to change the set position X is also used to change the set position
Luuk 4:2786719c73fd 46 volatile double Y, X;
Luuk 4:2786719c73fd 47
Luuk 3:03db694efdbe 48 volatile bool ReturnIP;
Luuk 3:03db694efdbe 49 int counter; //to count the time of inactivity
Luuk 3:03db694efdbe 50
Luuk 4:2786719c73fd 51 double emg1filt(double X)
Luuk 4:2786719c73fd 52 {
Luuk 4:2786719c73fd 53 double xtemp=(bq2.step(fabs(bq1.step(emg1))));
Luuk 4:2786719c73fd 54 if (xtemp>a)
Luuk 4:2786719c73fd 55 a=xtemp;
Luuk 4:2786719c73fd 56
Luuk 4:2786719c73fd 57 X = xtemp/a;
Luuk 4:2786719c73fd 58 return X;
Luuk 4:2786719c73fd 59 }
Luuk 4:2786719c73fd 60
Luuk 4:2786719c73fd 61 double emg2filt(double Y1)
Luuk 4:2786719c73fd 62 {
Luuk 4:2786719c73fd 63 double y1temp = (bq2.step(fabs(bq1.step(emg1))));
Luuk 4:2786719c73fd 64 if (y1temp>b)
Luuk 4:2786719c73fd 65 b=y1temp;
Luuk 4:2786719c73fd 66
Luuk 4:2786719c73fd 67 Y1 = y1temp/b;
Luuk 4:2786719c73fd 68 return Y1;
Luuk 4:2786719c73fd 69 }
Luuk 4:2786719c73fd 70
Luuk 4:2786719c73fd 71 int emg3filt(int Y2)
Luuk 4:2786719c73fd 72 {
Luuk 4:2786719c73fd 73 double y2temp = (bq2.step(fabs(bq1.step(emg1))));
Luuk 4:2786719c73fd 74 if (y2temp>c)
Luuk 4:2786719c73fd 75 {
Luuk 4:2786719c73fd 76 c=y2temp;
Luuk 4:2786719c73fd 77 }
Luuk 4:2786719c73fd 78 Y2 = y2temp/c;
Luuk 4:2786719c73fd 79 if(Y1 < 0.3)
Luuk 4:2786719c73fd 80 Y2 = 1;
Luuk 4:2786719c73fd 81 else
Luuk 4:2786719c73fd 82 Y2 = -1;
Luuk 4:2786719c73fd 83 return Y2;
Luuk 4:2786719c73fd 84 }
Luuk 4:2786719c73fd 85
Luuk 4:2786719c73fd 86 void callemgfilt()
Luuk 4:2786719c73fd 87 {
Luuk 4:2786719c73fd 88 X = emg1filt(X);
Luuk 4:2786719c73fd 89 Y = emg2filt(Y1)*emg3filt(Y2); // emg2filt gives the amplitude of Y and emg3filt the sign
Luuk 4:2786719c73fd 90
Luuk 4:2786719c73fd 91 }
Luuk 4:2786719c73fd 92
Luuk 3:03db694efdbe 93 // PID controller (Tustin approximation)
Luuk 3:03db694efdbe 94 float PID(float err, const float KP, const float KI, const float KD,const float Ts, const float N, float &v1, float &v2)
Luuk 3:03db694efdbe 95 {
Luuk 3:03db694efdbe 96 const float a1 = -(N*Ts+2);
Luuk 3:03db694efdbe 97 const float a2 = -(N*Ts-2)/(N*Ts+2);
Luuk 3:03db694efdbe 98 const float b0 = (4*KP+4*KD*N+2*KI*Ts+2*KP*N*Ts+KI*N*pow(Ts,2))/(2*N*Ts+4);
Luuk 3:03db694efdbe 99 const float b1 = (KI*N*pow(Ts,2)-4*KP-4*KD*N)/(N*Ts+2);
Luuk 3:03db694efdbe 100 const float b2 = (4*KP+4*KD*N-2*KI*Ts-2*KP*N*pow(Ts,2))/(2*N*Ts+4);
Luuk 3:03db694efdbe 101
Luuk 3:03db694efdbe 102 float v = err-a1*v1-a2*v2;
Luuk 3:03db694efdbe 103 float u = abs(b0*v+b1*v1+b2*v2);
Luuk 3:03db694efdbe 104 v2 = v1; v1 = v;
Luuk 3:03db694efdbe 105 return u;
Luuk 3:03db694efdbe 106 }
Luuk 0:1b8a08e9a66c 107
Luuk 3:03db694efdbe 108 void positionxy(float Angle1, float Angle2,float &XPos,float &YPos)
Luuk 3:03db694efdbe 109 {
Luuk 3:03db694efdbe 110 Angle1 = (encoder1.getPulses()/(2*PulsesPerRev*GearRatio))*2*pi + Angle1_0; //angles of the arms driven by the motors in radians
Luuk 3:03db694efdbe 111 Angle2 = (encoder2.getPulses()/(2*PulsesPerRev*GearRatio))*2*pi + Angle2_0;
Luuk 3:03db694efdbe 112
Luuk 3:03db694efdbe 113 XPos = L*cos(Angle1)+L*cos(Angle2); // calculate the position of the end-effector
Luuk 3:03db694efdbe 114 YPos = L*sin(Angle1)+L*sin(Angle2);
Luuk 3:03db694efdbe 115 }
Luuk 0:1b8a08e9a66c 116
Luuk 3:03db694efdbe 117 int Direction(float err)
Luuk 3:03db694efdbe 118 { //choose the direction of the motor, using the difference between the set angle and the actual angle.
Luuk 3:03db694efdbe 119 int a;
Luuk 3:03db694efdbe 120 if(err >= 0)
Luuk 3:03db694efdbe 121 a = 1;
Luuk 3:03db694efdbe 122 else
Luuk 3:03db694efdbe 123 a = 0;
Luuk 3:03db694efdbe 124 return a;
Luuk 3:03db694efdbe 125 }
Luuk 3:03db694efdbe 126
Luuk 3:03db694efdbe 127 void MoveMotors() // move the motors using the inverse kinematic equations of the robot
Luuk 3:03db694efdbe 128 {
Luuk 3:03db694efdbe 129 float VX = KV*(XSet - XPos); //set the desired velocity, proportional to the difference between the set point and the end-effector position.
Luuk 3:03db694efdbe 130 float VY = KV*(YSet-YPos);
Luuk 3:03db694efdbe 131
Luuk 3:03db694efdbe 132 float W1 = (VX*(XPos-L*cos(Angle1))+VY*(YPos-L*sin(Angle1)))/(L*YPos*cos(Angle1)-L*XPos*sin(Angle1)); // calculate the needed angular velocity to achieve the desired velocity
Luuk 3:03db694efdbe 133 float W2 = (-VX*XPos-VY*YPos)/(L*YPos*cos(Angle1)-L*XPos*sin(Angle1));
Luuk 3:03db694efdbe 134
Luuk 3:03db694efdbe 135 float Angle1Set = Angle1 + W1*Ts; //calculate the set angle, angle at which the motor should be after 1 period with the calculated angular velocity
Luuk 3:03db694efdbe 136 float Angle2Set = Angle2 + W2*Ts;
Luuk 3:03db694efdbe 137
Luuk 3:03db694efdbe 138 Motor1Vel = PID(Angle1Set-Angle1,KP,KI,KD,Ts,N,M1_v1,M1_v2); //PID controller to choose the velocity of the motors
Luuk 3:03db694efdbe 139 Motor1Dir = Direction(Angle1Set-Angle1);
Luuk 3:03db694efdbe 140 Motor2Vel = PID(Angle2Set-Angle2,KP,KI,KD,Ts,N,M2_v1,M2_v2);
Luuk 3:03db694efdbe 141 Motor2Dir = Direction(Angle2Set-Angle2);
Luuk 2:e36213affbda 142 }
Luuk 0:1b8a08e9a66c 143
Luuk 3:03db694efdbe 144 void GoToSet()
Luuk 3:03db694efdbe 145 {
Luuk 3:03db694efdbe 146 XSet += X; // keep increasing the set point, depending on the input
Luuk 3:03db694efdbe 147 YSet += Y;
Luuk 3:03db694efdbe 148 positionxy(Angle1,Angle2,XPos,YPos);
Luuk 3:03db694efdbe 149 MoveMotors();
Luuk 0:1b8a08e9a66c 150 }
Luuk 4:2786719c73fd 151
Luuk 3:03db694efdbe 152 void InitialPosition() //Go back to the initial position
Luuk 3:03db694efdbe 153 {
Luuk 4:2786719c73fd 154 const float AllowedError = 0.5;
Luuk 4:2786719c73fd 155 const float X0 = 14.5, X1 = 21.85, X2 = 31.94, slope = -17.34/6.6; //values of special points in the workspace
Luuk 3:03db694efdbe 156
Luuk 3:03db694efdbe 157 positionxy(Angle1,Angle2,XPos,YPos);
Luuk 3:03db694efdbe 158 if(sqrt(pow(XPos-XSet,2)+pow(YPos-YSet,2))<AllowedError) //to call the function until the end effector is at the initial position
Luuk 3:03db694efdbe 159 {
Luuk 3:03db694efdbe 160 ReturnIP = false;
Luuk 3:03db694efdbe 161 }
Luuk 3:03db694efdbe 162 else
Luuk 2:e36213affbda 163 {
Luuk 3:03db694efdbe 164 ReturnIP = true;
Luuk 3:03db694efdbe 165 }
Luuk 3:03db694efdbe 166 //The following if-else statement is to choose a set point so that the end-effector does not go out of the work space while returning
Luuk 3:03db694efdbe 167 if(XPos<X1)
Luuk 3:03db694efdbe 168 {
Luuk 3:03db694efdbe 169 XSet = X0;
Luuk 3:03db694efdbe 170 YSet = 0;
Luuk 2:e36213affbda 171 }
Luuk 3:03db694efdbe 172 else if(XPos < X2 && (XPos < slope*YPos+X0 || XPos < slope*YPos+X0 ))
Luuk 3:03db694efdbe 173 {
Luuk 3:03db694efdbe 174 XSet = XPos;
Luuk 3:03db694efdbe 175 YSet = 0;
Luuk 3:03db694efdbe 176 }
Luuk 3:03db694efdbe 177 else if(XPos >= X2 && (XPos < slope*YPos+X0 || XPos < slope*YPos+X0))
Luuk 3:03db694efdbe 178 {
Luuk 3:03db694efdbe 179 XSet = X2;
Luuk 3:03db694efdbe 180 YSet = 0;
Luuk 3:03db694efdbe 181 }
Luuk 3:03db694efdbe 182 else
Luuk 2:e36213affbda 183 {
Luuk 3:03db694efdbe 184 XSet = X0;
Luuk 3:03db694efdbe 185 YSet = 0;
Luuk 2:e36213affbda 186 }
Luuk 3:03db694efdbe 187 MoveMotors();
Luuk 0:1b8a08e9a66c 188 }
Luuk 4:2786719c73fd 189
Luuk 3:03db694efdbe 190 void CallFuncMovement() //decide which case has to be used
Luuk 3:03db694efdbe 191 {
Luuk 3:03db694efdbe 192 positionxy(Angle1,Angle2,XPos,YPos); //calculate the position of the end point
Luuk 3:03db694efdbe 193 if (Button == 1)
Luuk 3:03db694efdbe 194 { //stop motors
Luuk 3:03db694efdbe 195 Motor1Vel = 0;
Luuk 3:03db694efdbe 196 Motor2Vel = 0;
Luuk 3:03db694efdbe 197 }
Luuk 3:03db694efdbe 198 else if (ReturnIP)
Luuk 3:03db694efdbe 199 { //when InitialPosition is called, keep calling until the end-effector gets there;
Luuk 3:03db694efdbe 200 InitialPosition();
Luuk 3:03db694efdbe 201 counter = 0;
Luuk 3:03db694efdbe 202 }
Luuk 3:03db694efdbe 203 else if (sqrt(pow(XPos-300,2) + pow(YPos,2)) > 280 || sqrt(pow(XPos,2) + pow(YPos,2)) > 570 || sqrt(pow(XPos,2) + pow(YPos-300,2)) > 320 || sqrt(pow(XPos,2) + pow(YPos+300,2)) > 320 )
Luuk 3:03db694efdbe 204 { // limit so that the robot does not break.
Luuk 3:03db694efdbe 205 InitialPosition();
Luuk 3:03db694efdbe 206 counter = 0;
Luuk 3:03db694efdbe 207 }
Luuk 3:03db694efdbe 208 else if (counter == 2/Ts)
Luuk 3:03db694efdbe 209 { //after 2 seconds of inactivity the robot goes back to the initial position
Luuk 3:03db694efdbe 210 InitialPosition();
Luuk 3:03db694efdbe 211 counter = 0;
Luuk 3:03db694efdbe 212 }
Luuk 3:03db694efdbe 213 else if (X > 0 || Y != 0)
Luuk 0:1b8a08e9a66c 214 {
Luuk 3:03db694efdbe 215 GoToSet();
Luuk 3:03db694efdbe 216 counter = 0;
Luuk 0:1b8a08e9a66c 217 }
Luuk 3:03db694efdbe 218 else
Luuk 3:03db694efdbe 219 {
Luuk 3:03db694efdbe 220 counter++;
Luuk 3:03db694efdbe 221 }
Luuk 0:1b8a08e9a66c 222 }
Luuk 3:03db694efdbe 223
Luuk 0:1b8a08e9a66c 224 main()
Luuk 0:1b8a08e9a66c 225 {
Luuk 0:1b8a08e9a66c 226 pc.baud(115200);
Luuk 4:2786719c73fd 227 emg_tick.attach(&callemgfilt,0.001);
Luuk 4:2786719c73fd 228 motors_tick.attach(&CallFuncMovement,Ts);
Luuk 2:e36213affbda 229 while (true)
Luuk 4:2786719c73fd 230 { /*keep the program running*/ }
Luuk 0:1b8a08e9a66c 231 }