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@3:03db694efdbe, 2017-10-26 (annotated)

Committer:
Luuk
Date:
Thu Oct 26 21:07:45 2017 +0000
Revision:
3:03db694efdbe
Parent:
2:e36213affbda
Child:
4:2786719c73fd
Motor control program, without emg input and with inverse kinematic equations implemented

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 0:1b8a08e9a66c 3 #include "MODSERIAL.h"
Luuk 0:1b8a08e9a66c 4 //#include "HIDScope.h"
Luuk 0:1b8a08e9a66c 5
Luuk 3:03db694efdbe 6 //there is something wrong with the code, the motor turns twice when the potmeters are at the minimum
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 2:e36213affbda 16 DigitalOut Button(D3);
Luuk 0:1b8a08e9a66c 17 //DigitalOut ledb(LED1);
Luuk 0:1b8a08e9a66c 18
Luuk 1:14b685c3abbd 19 QEI encoder1(D8,D9,NC,16);
Luuk 1:14b685c3abbd 20 QEI encoder2(D10,D11,NC,16);
Luuk 1:14b685c3abbd 21
Luuk 0:1b8a08e9a66c 22 Ticker switch_tick;
Luuk 3:03db694efdbe 23 float Angle1,Angle2; //real angles of the motor
Luuk 3:03db694efdbe 24 float XPos, YPos; //position of the end-effector
Luuk 3:03db694efdbe 25 float XSet, YSet; //desired position of the end-effector
Luuk 3:03db694efdbe 26 float L = 300.0; //length of the arms of the robot
Luuk 3:03db694efdbe 27 const float Ts = 0.01; //to control the tickers
Luuk 3:03db694efdbe 28 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 29 const float KV = 1; //velocity constant, to scale the desired velocity of the end-effector
Luuk 3:03db694efdbe 30 float X,Y; //emg input for the motors
Luuk 3:03db694efdbe 31
Luuk 3:03db694efdbe 32 const float pi = 3.14159265359,PulsesPerRev = 16.0,GearRatio = 131.15; //to convert pulses to radians
Luuk 3:03db694efdbe 33 const float Angle1_0 = pi/2 ,Angle2_0 = 0; //initial angle of the motors
Luuk 3:03db694efdbe 34
Luuk 3:03db694efdbe 35 //variables and constants for the PID
Luuk 3:03db694efdbe 36 const float KP = 10, KI = 5, KD = 2, N = 100;
Luuk 3:03db694efdbe 37 float M1_v1 = 0, M1_v2 = 0, M2_v1 = 0, M2_v2 = 0;
Luuk 3:03db694efdbe 38
Luuk 3:03db694efdbe 39 volatile bool ReturnIP;
Luuk 3:03db694efdbe 40 int counter; //to count the time of inactivity
Luuk 3:03db694efdbe 41
Luuk 3:03db694efdbe 42 // PID controller (Tustin approximation)
Luuk 3:03db694efdbe 43 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 44 {
Luuk 3:03db694efdbe 45 const float a1 = -(N*Ts+2);
Luuk 3:03db694efdbe 46 const float a2 = -(N*Ts-2)/(N*Ts+2);
Luuk 3:03db694efdbe 47 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 48 const float b1 = (KI*N*pow(Ts,2)-4*KP-4*KD*N)/(N*Ts+2);
Luuk 3:03db694efdbe 49 const float b2 = (4*KP+4*KD*N-2*KI*Ts-2*KP*N*pow(Ts,2))/(2*N*Ts+4);
Luuk 3:03db694efdbe 50
Luuk 3:03db694efdbe 51 float v = err-a1*v1-a2*v2;
Luuk 3:03db694efdbe 52 float u = abs(b0*v+b1*v1+b2*v2);
Luuk 3:03db694efdbe 53 v2 = v1; v1 = v;
Luuk 3:03db694efdbe 54 return u;
Luuk 3:03db694efdbe 55 }
Luuk 0:1b8a08e9a66c 56
Luuk 3:03db694efdbe 57 void positionxy(float Angle1, float Angle2,float &XPos,float &YPos)
Luuk 3:03db694efdbe 58 {
Luuk 3:03db694efdbe 59 Angle1 = (encoder1.getPulses()/(2*PulsesPerRev*GearRatio))*2*pi + Angle1_0; //angles of the arms driven by the motors in radians
Luuk 3:03db694efdbe 60 Angle2 = (encoder2.getPulses()/(2*PulsesPerRev*GearRatio))*2*pi + Angle2_0;
Luuk 3:03db694efdbe 61
Luuk 3:03db694efdbe 62 XPos = L*cos(Angle1)+L*cos(Angle2); // calculate the position of the end-effector
Luuk 3:03db694efdbe 63 YPos = L*sin(Angle1)+L*sin(Angle2);
Luuk 3:03db694efdbe 64 }
Luuk 0:1b8a08e9a66c 65
Luuk 3:03db694efdbe 66 int Direction(float err)
Luuk 3:03db694efdbe 67 { //choose the direction of the motor, using the difference between the set angle and the actual angle.
Luuk 3:03db694efdbe 68 int a;
Luuk 3:03db694efdbe 69 if(err >= 0)
Luuk 3:03db694efdbe 70 a = 1;
Luuk 3:03db694efdbe 71 else
Luuk 3:03db694efdbe 72 a = 0;
Luuk 3:03db694efdbe 73 return a;
Luuk 3:03db694efdbe 74 }
Luuk 3:03db694efdbe 75
Luuk 3:03db694efdbe 76 void MoveMotors() // move the motors using the inverse kinematic equations of the robot
Luuk 3:03db694efdbe 77 {
Luuk 3:03db694efdbe 78 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 79 float VY = KV*(YSet-YPos);
Luuk 3:03db694efdbe 80
Luuk 3:03db694efdbe 81 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 82 float W2 = (-VX*XPos-VY*YPos)/(L*YPos*cos(Angle1)-L*XPos*sin(Angle1));
Luuk 3:03db694efdbe 83
Luuk 3:03db694efdbe 84 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 85 float Angle2Set = Angle2 + W2*Ts;
Luuk 3:03db694efdbe 86
Luuk 3:03db694efdbe 87 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 88 Motor1Dir = Direction(Angle1Set-Angle1);
Luuk 3:03db694efdbe 89 Motor2Vel = PID(Angle2Set-Angle2,KP,KI,KD,Ts,N,M2_v1,M2_v2);
Luuk 3:03db694efdbe 90 Motor2Dir = Direction(Angle2Set-Angle2);
Luuk 2:e36213affbda 91 }
Luuk 0:1b8a08e9a66c 92
Luuk 3:03db694efdbe 93 void GoToSet()
Luuk 3:03db694efdbe 94 {
Luuk 3:03db694efdbe 95 XSet += X; // keep increasing the set point, depending on the input
Luuk 3:03db694efdbe 96 YSet += Y;
Luuk 3:03db694efdbe 97 positionxy(Angle1,Angle2,XPos,YPos);
Luuk 3:03db694efdbe 98 MoveMotors();
Luuk 0:1b8a08e9a66c 99 }
Luuk 3:03db694efdbe 100 void InitialPosition() //Go back to the initial position
Luuk 3:03db694efdbe 101 {
Luuk 3:03db694efdbe 102 const float AllowedError = 1;
Luuk 3:03db694efdbe 103
Luuk 3:03db694efdbe 104 positionxy(Angle1,Angle2,XPos,YPos);
Luuk 3:03db694efdbe 105 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 106 {
Luuk 3:03db694efdbe 107 ReturnIP = false;
Luuk 3:03db694efdbe 108 }
Luuk 3:03db694efdbe 109 else
Luuk 2:e36213affbda 110 {
Luuk 3:03db694efdbe 111 ReturnIP = true;
Luuk 3:03db694efdbe 112 }
Luuk 3:03db694efdbe 113 //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 114 if(XPos<X1)
Luuk 3:03db694efdbe 115 {
Luuk 3:03db694efdbe 116 XSet = X0;
Luuk 3:03db694efdbe 117 YSet = 0;
Luuk 2:e36213affbda 118 }
Luuk 3:03db694efdbe 119 else if(XPos < X2 && (XPos < slope*YPos+X0 || XPos < slope*YPos+X0 ))
Luuk 3:03db694efdbe 120 {
Luuk 3:03db694efdbe 121 XSet = XPos;
Luuk 3:03db694efdbe 122 YSet = 0;
Luuk 3:03db694efdbe 123 }
Luuk 3:03db694efdbe 124 else if(XPos >= X2 && (XPos < slope*YPos+X0 || XPos < slope*YPos+X0))
Luuk 3:03db694efdbe 125 {
Luuk 3:03db694efdbe 126 XSet = X2;
Luuk 3:03db694efdbe 127 YSet = 0;
Luuk 3:03db694efdbe 128 }
Luuk 3:03db694efdbe 129 else
Luuk 2:e36213affbda 130 {
Luuk 3:03db694efdbe 131 XSet = X0;
Luuk 3:03db694efdbe 132 YSet = 0;
Luuk 2:e36213affbda 133 }
Luuk 3:03db694efdbe 134 MoveMotors();
Luuk 0:1b8a08e9a66c 135 }
Luuk 3:03db694efdbe 136 void CallFuncMovement() //decide which case has to be used
Luuk 3:03db694efdbe 137 {
Luuk 3:03db694efdbe 138 positionxy(Angle1,Angle2,XPos,YPos); //calculate the position of the end point
Luuk 3:03db694efdbe 139 if (Button == 1)
Luuk 3:03db694efdbe 140 { //stop motors
Luuk 3:03db694efdbe 141 Motor1Vel = 0;
Luuk 3:03db694efdbe 142 Motor2Vel = 0;
Luuk 3:03db694efdbe 143 }
Luuk 3:03db694efdbe 144 else if (ReturnIP)
Luuk 3:03db694efdbe 145 { //when InitialPosition is called, keep calling until the end-effector gets there;
Luuk 3:03db694efdbe 146 InitialPosition();
Luuk 3:03db694efdbe 147 counter = 0;
Luuk 3:03db694efdbe 148 }
Luuk 3:03db694efdbe 149 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 150 { // limit so that the robot does not break.
Luuk 3:03db694efdbe 151 InitialPosition();
Luuk 3:03db694efdbe 152 counter = 0;
Luuk 3:03db694efdbe 153 }
Luuk 3:03db694efdbe 154 else if (counter == 2/Ts)
Luuk 3:03db694efdbe 155 { //after 2 seconds of inactivity the robot goes back to the initial position
Luuk 3:03db694efdbe 156 InitialPosition();
Luuk 3:03db694efdbe 157 counter = 0;
Luuk 3:03db694efdbe 158 }
Luuk 3:03db694efdbe 159 else if (X > 0 || Y != 0)
Luuk 0:1b8a08e9a66c 160 {
Luuk 3:03db694efdbe 161 GoToSet();
Luuk 3:03db694efdbe 162 counter = 0;
Luuk 0:1b8a08e9a66c 163 }
Luuk 3:03db694efdbe 164 else
Luuk 3:03db694efdbe 165 {
Luuk 3:03db694efdbe 166 counter++;
Luuk 3:03db694efdbe 167 }
Luuk 0:1b8a08e9a66c 168 }
Luuk 3:03db694efdbe 169
Luuk 3:03db694efdbe 170
Luuk 0:1b8a08e9a66c 171 main()
Luuk 0:1b8a08e9a66c 172 {
Luuk 0:1b8a08e9a66c 173 pc.baud(115200);
Luuk 3:03db694efdbe 174 switch_tick.attach(&CallFuncMovement,Ts);
Luuk 2:e36213affbda 175 while (true)
Luuk 2:e36213affbda 176 {
Luuk 0:1b8a08e9a66c 177 }
Luuk 0:1b8a08e9a66c 178 }