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

Committer:
Luuk
Date:
Mon Oct 30 15:03:53 2017 +0000
Revision:
5:77ffa336d6eb
Parent:
4:2786719c73fd
Child:
6:acc2669ab20c
hey

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