Luuk Lomillos Rozeboom / Mbed 2 deprecated RobotControlFinal

Robot control

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

main.cpp@7:75d2244d7745, 2017-10-31 (annotated)

Committer:
Luuk
Date:
Tue Oct 31 11:21:16 2017 +0000
Revision:
7:75d2244d7745
Parent:
6:acc2669ab20c 
with calibration

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