Silvie van den Bogaard / Mbed 2 deprecated demo_mode

Knopjes kunnen gebruikt worden om de robot the bedienen

Dependencies:   FastPWM MODSERIAL QEI mbed

main.cpp@0:f81ca7708f95, 2018-11-01 (annotated)

Committer:
Duif
Date:
Thu Nov 01 11:03:18 2018 +0000
Revision:
0:f81ca7708f95
Child:
1:f628ab39b9f4
Demo mode waarbij de knopjes de robot bedienen

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Duif 0:f81ca7708f95 1 #include "mbed.h"
Duif 0:f81ca7708f95 2 #include "FastPWM.h"
Duif 0:f81ca7708f95 3 #include "MODSERIAL.h"
Duif 0:f81ca7708f95 4 #include "QEI.h"
Duif 0:f81ca7708f95 5 #include "math.h"
Duif 0:f81ca7708f95 6
Duif 0:f81ca7708f95 7 // --------------------------------------------------
Duif 0:f81ca7708f95 8 // ----------------- SET UP -------------------------
Duif 0:f81ca7708f95 9 QEI Encoder1(D11, D10, NC, 4200) ; // Encoder motor 1, (pin 1A, pin 1B, counts/rev)
Duif 0:f81ca7708f95 10 QEI Encoder2(D9, D8, NC, 4200) ; // Encoder motor 2, (pin 2A, pin 2B, counts/rev)
Duif 0:f81ca7708f95 11 DigitalOut directionM1(D4);
Duif 0:f81ca7708f95 12 DigitalOut directionM2(D7);
Duif 0:f81ca7708f95 13 FastPWM motor1_pwm(D5);
Duif 0:f81ca7708f95 14 FastPWM motor2_pwm(D6);
Duif 0:f81ca7708f95 15 MODSERIAL pc(USBTX, USBRX);
Duif 0:f81ca7708f95 16 DigitalOut ledr(LED_RED);
Duif 0:f81ca7708f95 17 DigitalOut ledg(LED_GREEN);
Duif 0:f81ca7708f95 18 DigitalOut ledb(LED_BLUE);
Duif 0:f81ca7708f95 19 DigitalIn directionx(SW3); //x direction switch button
Duif 0:f81ca7708f95 20 DigitalIn directiony(SW2); //y direction switch button
Duif 0:f81ca7708f95 21 DigitalIn buttonx(D2); //x EMG replacement
Duif 0:f81ca7708f95 22 DigitalIn buttony(D3); //y EMG replacement
Duif 0:f81ca7708f95 23
Duif 0:f81ca7708f95 24 // Tickers
Duif 0:f81ca7708f95 25 Ticker Demo;
Duif 0:f81ca7708f95 26 Ticker Write;
Duif 0:f81ca7708f95 27
Duif 0:f81ca7708f95 28 // ---------------------------------------------------
Duif 0:f81ca7708f95 29 // ----------------- GLOBAL VARIABLES ----------------
Duif 0:f81ca7708f95 30 volatile int counts1;
Duif 0:f81ca7708f95 31 volatile int counts2;
Duif 0:f81ca7708f95 32 volatile double theta1;
Duif 0:f81ca7708f95 33 volatile double theta2;
Duif 0:f81ca7708f95 34 const double pi = 3.14159265359;
Duif 0:f81ca7708f95 35 volatile double error1;
Duif 0:f81ca7708f95 36 volatile double error2;
Duif 0:f81ca7708f95 37 //volatile double error1_final=10.0;
Duif 0:f81ca7708f95 38 //volatile double error2_final=10.0;
Duif 0:f81ca7708f95 39 //volatile double q1_diff_final;
Duif 0:f81ca7708f95 40 //volatile double q2_diff_final;
Duif 0:f81ca7708f95 41 //double point1x = 200.0;
Duif 0:f81ca7708f95 42 //double point1y = 200.0;
Duif 0:f81ca7708f95 43 //double point2x = 200.0;
Duif 0:f81ca7708f95 44 //double point2y = 100.0;
Duif 0:f81ca7708f95 45 //double point3x = 350.0;
Duif 0:f81ca7708f95 46 //double point3y = 0.0;
Duif 0:f81ca7708f95 47 //double point4x = 200.0;
Duif 0:f81ca7708f95 48 //double point4y = 0.0;
Duif 0:f81ca7708f95 49 //volatile int track = 1;
Duif 0:f81ca7708f95 50 const double x0 = 80.0; //zero x position after homing
Duif 0:f81ca7708f95 51 const double y0 = 141.0; //zero y position after homing
Duif 0:f81ca7708f95 52 volatile double setpointx = x0;
Duif 0:f81ca7708f95 53 volatile double setpointy = y0;
Duif 0:f81ca7708f95 54 volatile double U1;
Duif 0:f81ca7708f95 55 volatile double U2;
Duif 0:f81ca7708f95 56 volatile double xfinal;
Duif 0:f81ca7708f95 57 volatile double yfinal;
Duif 0:f81ca7708f95 58 volatile int sx;//value of the button and store as switch
Duif 0:f81ca7708f95 59 volatile int sy;//value of the button and store as switch
Duif 0:f81ca7708f95 60 double dirx = 1.0; //determine the direction of the setpoint placement
Duif 0:f81ca7708f95 61 double diry = 1.0; //determine the direction of the setpoint placement
Duif 0:f81ca7708f95 62
Duif 0:f81ca7708f95 63 const double v=0.1; //moving speed of setpoint
Duif 0:f81ca7708f95 64
Duif 0:f81ca7708f95 65
Duif 0:f81ca7708f95 66 // Inverse Kinematics
Duif 0:f81ca7708f95 67 volatile double q1_diff;
Duif 0:f81ca7708f95 68 volatile double q2_diff;
Duif 0:f81ca7708f95 69 const double sq = 2.0; //to square numbers
Duif 0:f81ca7708f95 70 const double L1 = 250.0; //length of the first link
Duif 0:f81ca7708f95 71 const double L3 = 350.0; //length of the second link
Duif 0:f81ca7708f95 72
Duif 0:f81ca7708f95 73 // Reference angles of the starting position
Duif 0:f81ca7708f95 74 double q2_0 = -acos((pow(x0,sq)+pow(y0,sq)-pow(L1,sq)-pow(L3,sq))/(2.0*L1*L3));
Duif 0:f81ca7708f95 75 double q1_0 = atan(y0/x0)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x0,sq)+pow(y0,sq))/(2.0*L1*sqrt(pow(x0,sq)+pow(y0,sq))));
Duif 0:f81ca7708f95 76 double q2_0_enc = (pi-q2_0) + q1_0;
Duif 0:f81ca7708f95 77
Duif 0:f81ca7708f95 78 // --------------------------------------------------------------------
Duif 0:f81ca7708f95 79 // ---------------Read out encoders------------------------------------
Duif 0:f81ca7708f95 80 // --------------------------------------------------------------------
Duif 0:f81ca7708f95 81 double counts2angle1()
Duif 0:f81ca7708f95 82 {
Duif 0:f81ca7708f95 83 counts1 = Encoder1.getPulses(); // Counts of outputshaft of motor 1
Duif 0:f81ca7708f95 84 theta1 = -(double(counts1)/4200) * 2*pi; // Angle of outputshaft of motor 1
Duif 0:f81ca7708f95 85 return theta1;
Duif 0:f81ca7708f95 86 }
Duif 0:f81ca7708f95 87
Duif 0:f81ca7708f95 88 double counts2angle2()
Duif 0:f81ca7708f95 89 {
Duif 0:f81ca7708f95 90 counts2 = Encoder2.getPulses(); // Counts of outputshaft of motor 2
Duif 0:f81ca7708f95 91 theta2 = (double(counts2)/4200) * 2*pi; // Angle of outputshaft of motor 2
Duif 0:f81ca7708f95 92 return theta2;
Duif 0:f81ca7708f95 93 }
Duif 0:f81ca7708f95 94
Duif 0:f81ca7708f95 95 // -------------------------------------------------------------------------
Duif 0:f81ca7708f95 96 // -------------- Determine Setpoints --------------------------------------
Duif 0:f81ca7708f95 97 // -------------------------------------------------------------------------
Duif 0:f81ca7708f95 98
Duif 0:f81ca7708f95 99 void determinedemoset(){
Duif 0:f81ca7708f95 100 setpointx = setpointx + dirx*sx*v;
Duif 0:f81ca7708f95 101 setpointy = setpointy + diry*sy*v;
Duif 0:f81ca7708f95 102 }
Duif 0:f81ca7708f95 103
Duif 0:f81ca7708f95 104 //function to change the moving direction of the setpoint
Duif 0:f81ca7708f95 105 void ChangeDirectionX(){
Duif 0:f81ca7708f95 106 dirx = -1*dirx;
Duif 0:f81ca7708f95 107 }
Duif 0:f81ca7708f95 108
Duif 0:f81ca7708f95 109 void ChangeDirectionY(){
Duif 0:f81ca7708f95 110 diry = -1*diry;
Duif 0:f81ca7708f95 111 }
Duif 0:f81ca7708f95 112
Duif 0:f81ca7708f95 113
Duif 0:f81ca7708f95 114
Duif 0:f81ca7708f95 115 // -----------------------------------------------------------------
Duif 0:f81ca7708f95 116 // --------------------------- PI controllers ----------------------
Duif 0:f81ca7708f95 117 // -----------------------------------------------------------------
Duif 0:f81ca7708f95 118 double PI_controller1(double error1)
Duif 0:f81ca7708f95 119 {
Duif 0:f81ca7708f95 120 static double error_integral1 = 0;
Duif 0:f81ca7708f95 121
Duif 0:f81ca7708f95 122 // Proportional part
Duif 0:f81ca7708f95 123 double Kp1 = 3.95; // Kp (proportionele controller, nu nog een random waarde)
Duif 0:f81ca7708f95 124 double u_p1 = Kp1*error1; // Voltage dat naar de motor gestuurd wordt (volgt uit error en Kp)
Duif 0:f81ca7708f95 125
Duif 0:f81ca7708f95 126 // Integral part
Duif 0:f81ca7708f95 127 double Ki1 = 6.0; // Ki (Integrale deel vd controller, nu nog een random waarde)
Duif 0:f81ca7708f95 128 double Ts1 = 0.005; // Sample tijd, net zo vaak als de controller wordt aangeroepen (200 Hz, statemachine)
Duif 0:f81ca7708f95 129 error_integral1 = error_integral1 + error1 * Ts1;
Duif 0:f81ca7708f95 130 double u_i1 = Ki1 * error_integral1;
Duif 0:f81ca7708f95 131
Duif 0:f81ca7708f95 132 // Sum
Duif 0:f81ca7708f95 133 U1 = u_p1 + u_i1;
Duif 0:f81ca7708f95 134
Duif 0:f81ca7708f95 135 // Return
Duif 0:f81ca7708f95 136 return U1;
Duif 0:f81ca7708f95 137 }
Duif 0:f81ca7708f95 138 double PI_controller2(double error2)
Duif 0:f81ca7708f95 139 {
Duif 0:f81ca7708f95 140 static double error_integral2 = 0;
Duif 0:f81ca7708f95 141
Duif 0:f81ca7708f95 142 // Proportional part
Duif 0:f81ca7708f95 143 double Kp2 = 3.95; // Kp (proportionele controller, nu nog een random waarde)
Duif 0:f81ca7708f95 144 double u_p2 = Kp2*error2; // Voltage dat naar de motor gestuurd wordt (volgt uit error en Kp)
Duif 0:f81ca7708f95 145
Duif 0:f81ca7708f95 146 // Integral part
Duif 0:f81ca7708f95 147 double Ki2 = 6.0; // Ki (Integrale deel vd controller, nu nog een random waarde)
Duif 0:f81ca7708f95 148 double Ts2 = 0.005; // Sample tijd, net zo vaak als de controller wordt aangeroepen (200 Hz, statemachine)
Duif 0:f81ca7708f95 149 error_integral2 = error_integral2 + error2 * Ts2;
Duif 0:f81ca7708f95 150 double u_i2 = Ki2 * error_integral2;
Duif 0:f81ca7708f95 151
Duif 0:f81ca7708f95 152 // Sum +
Duif 0:f81ca7708f95 153 U2 = u_p2 + u_i2;
Duif 0:f81ca7708f95 154
Duif 0:f81ca7708f95 155 // Return
Duif 0:f81ca7708f95 156 return U2;
Duif 0:f81ca7708f95 157 }
Duif 0:f81ca7708f95 158
Duif 0:f81ca7708f95 159 // ------------------------------------------------------------
Duif 0:f81ca7708f95 160 // ------------ Inverse Kinematics ----------------------------
Duif 0:f81ca7708f95 161 // ------------------------------------------------------------
Duif 0:f81ca7708f95 162 double makeAngleq1(double x, double y){
Duif 0:f81ca7708f95 163 double q1 = atan(y/x)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x,sq)+pow(y,sq))/(2.0*L1*sqrt(pow(x,sq)+pow(y,sq)))); //angle of the first joint in the setpoint configuration
Duif 0:f81ca7708f95 164 q1_diff = -(2.0*(q1-q1_0)); //the actual amount of radians that the motor has to turn in total to reach the setpoint
Duif 0:f81ca7708f95 165 return q1_diff;
Duif 0:f81ca7708f95 166 }
Duif 0:f81ca7708f95 167
Duif 0:f81ca7708f95 168 double makeAngleq2(double x, double y){
Duif 0:f81ca7708f95 169 double q2 = -acos((pow(x,sq)+pow(y,sq)-pow(L1,sq)-pow(L3,sq))/(2.0*L1*L3)); //angle of the second joint in setpoint configuration
Duif 0:f81ca7708f95 170 double q1 = atan(y/x)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x,sq)+pow(y,sq))/(2.0*L1*sqrt(pow(x,sq)+pow(y,sq)))); //angle of the first joint in the setpoint configuration
Duif 0:f81ca7708f95 171 double q2_motor = (pi - q2)+q1; //because q2 represents the angle at joint two and not at the motor a calculation has to be done
Duif 0:f81ca7708f95 172 q2_diff = -(2.0*(q2_motor - q2_0_enc))/(2.0*pi); //the actual amount of radians that the motor has to turn in total to reach the setpoint
Duif 0:f81ca7708f95 173 return q2_diff;
Duif 0:f81ca7708f95 174 }
Duif 0:f81ca7708f95 175
Duif 0:f81ca7708f95 176
Duif 0:f81ca7708f95 177 // -----------------------------------------------
Duif 0:f81ca7708f95 178 // ------------ RUN MOTORS -----------------------
Duif 0:f81ca7708f95 179 // -----------------------------------------------
Duif 0:f81ca7708f95 180 void motoraansturing()
Duif 0:f81ca7708f95 181 {
Duif 0:f81ca7708f95 182 determinedemoset();
Duif 0:f81ca7708f95 183 q1_diff = makeAngleq1(setpointx, setpointy);
Duif 0:f81ca7708f95 184 q2_diff = makeAngleq2(setpointx, setpointy);
Duif 0:f81ca7708f95 185 //q1_diff_final = makeAngleq1(xfinal, yfinal);
Duif 0:f81ca7708f95 186 //q2_diff_final = makeAngleq2(xfinal, yfinal);
Duif 0:f81ca7708f95 187
Duif 0:f81ca7708f95 188 theta2 = counts2angle2();
Duif 0:f81ca7708f95 189 error2 = q2_diff - theta2;
Duif 0:f81ca7708f95 190 theta1 = counts2angle1();
Duif 0:f81ca7708f95 191 error1 = q1_diff - theta1; // Setpoint error, te behalen setpoint minus de huidige positie van de as.
Duif 0:f81ca7708f95 192
Duif 0:f81ca7708f95 193 //errors die de setpoints bepalen
Duif 0:f81ca7708f95 194 //error1_final = q1_diff_final - theta1;
Duif 0:f81ca7708f95 195 //error2_final = q2_diff_final - theta2;
Duif 0:f81ca7708f95 196
Duif 0:f81ca7708f95 197 U1 = PI_controller1(error1); // Voltage dat naar de motor gestuurd wordt.
Duif 0:f81ca7708f95 198 U2 = PI_controller2(error2);
Duif 0:f81ca7708f95 199
Duif 0:f81ca7708f95 200 motor1_pwm.write(fabs(U1)); // Motor aansturen
Duif 0:f81ca7708f95 201 directionM1 = U1 > 0.0f; // Richting van de motor bepalen
Duif 0:f81ca7708f95 202 motor2_pwm.write(fabs(U2));
Duif 0:f81ca7708f95 203 directionM2 = U2 > 0.0f;
Duif 0:f81ca7708f95 204 }
Duif 0:f81ca7708f95 205
Duif 0:f81ca7708f95 206
Duif 0:f81ca7708f95 207 void rundemo()
Duif 0:f81ca7708f95 208 {
Duif 0:f81ca7708f95 209 motoraansturing();
Duif 0:f81ca7708f95 210 }
Duif 0:f81ca7708f95 211
Duif 0:f81ca7708f95 212
Duif 0:f81ca7708f95 213 int main()
Duif 0:f81ca7708f95 214 {
Duif 0:f81ca7708f95 215 pc.baud(115200);
Duif 0:f81ca7708f95 216 motor1_pwm.period_us(60); // Period is 60 microseconde
Duif 0:f81ca7708f95 217 motor2_pwm.period_us(60);
Duif 0:f81ca7708f95 218 Demo.attach(&rundemo, 0.005f);
Duif 0:f81ca7708f95 219
Duif 0:f81ca7708f95 220 InterruptIn directionx(SW3);
Duif 0:f81ca7708f95 221 directionx.fall(ChangeDirectionX); //change the direction of the setpoint in x direction
Duif 0:f81ca7708f95 222 InterruptIn directiony(SW2);
Duif 0:f81ca7708f95 223 directiony.fall(ChangeDirectionY); //change the direction of the setpoint in y direction
Duif 0:f81ca7708f95 224
Duif 0:f81ca7708f95 225 pc.printf("\r\n\r\nDOE HET AUB!!! \r\n\r\n");
Duif 0:f81ca7708f95 226
Duif 0:f81ca7708f95 227 while (true) {
Duif 0:f81ca7708f95 228 sx = !buttonx.read(); //this has to be replaced with the input from the EMG, this then functions like a button
Duif 0:f81ca7708f95 229 sy = !buttony.read(); //this has to be replaced with the input from the EMG, this then functions like a button
Duif 0:f81ca7708f95 230 pc.printf("Setpointx: %0.2f, Setpointy: %02f, q1_diff: %0.2f, q2_diff: %0.2f, error1: %0.2f, error2: %0.2f, U1: %0.2f, U2: %0.2f\r\n", setpointx,setpointy,q1_diff,q2_diff,error1,error2,U1,U2);
Duif 0:f81ca7708f95 231 //if (track == 1) {
Duif 0:f81ca7708f95 232 //pc.printf("Gaat naar positie 1\r\n");
Duif 0:f81ca7708f95 233 //}
Duif 0:f81ca7708f95 234 //else if (track == 12) {
Duif 0:f81ca7708f95 235 //pc.printf("Gaat naar positie 2\r\n");
Duif 0:f81ca7708f95 236 //}
Duif 0:f81ca7708f95 237 wait(0.5f);
Duif 0:f81ca7708f95 238
Duif 0:f81ca7708f95 239 }
Duif 0:f81ca7708f95 240 }