BMT Module 9 Group 4 / Mbed 2 deprecated DemoState

DemoState with IK and MotorControl and input vx_des and vy_des with potmeter1 and button2

Dependencies:   FastPWM MODSERIAL Matrix MatrixMath mbed QEI

main.cpp@4:854aa2e7eeb2, 2018-11-01 (annotated)

Committer:
1856413
Date:
Thu Nov 01 20:50:17 2018 +0000
Revision:
4:854aa2e7eeb2
Parent:
3:d16182dd3a2a
Child:
5:aca2af310419
Lees comments voor aanmerkingen

Who changed what in which revision?

UserRevisionLine numberNew contents of line
1856413 0:550f6e86da32 1 #include "mbed.h"
1856413 0:550f6e86da32 2 #include "FastPWM.h"
1856413 0:550f6e86da32 3 #include "MODSERIAL.h"
1856413 0:550f6e86da32 4 #include "Matrix.h"
1856413 0:550f6e86da32 5 #include "QEI.h"
1856413 0:550f6e86da32 6 #include <math.h>
1856413 0:550f6e86da32 7 MODSERIAL pc(USBTX, USBRX);
1856413 1:2219a519e2bf 8 DigitalOut motor1Direction(D7);
1856413 1:2219a519e2bf 9 DigitalOut motor2Direction(D4);
1856413 0:550f6e86da32 10 DigitalOut Led(LED_GREEN);
1856413 2:638c6155d0af 11 DigitalIn button2(SW3); //dit is nog button op mbed bor
1856413 1:2219a519e2bf 12 FastPWM motor1PWM(D6);
1856413 1:2219a519e2bf 13 FastPWM motor2PWM(D5);
1856413 0:550f6e86da32 14 AnalogIn potMeter1(A4);
1856413 0:550f6e86da32 15 AnalogIn potMeter2(A5);
1856413 0:550f6e86da32 16 QEI Encoder1 (D12, D13, NC, 64, QEI::X4_ENCODING);
1856413 0:550f6e86da32 17 QEI Encoder2 (D10, D11, NC, 64, QEI::X4_ENCODING);
1856413 0:550f6e86da32 18
1856413 0:550f6e86da32 19 // Tickers
1856413 2:638c6155d0af 20 Ticker potmeterTicker;
1856413 4:854aa2e7eeb2 21 Ticker allesineenticker;
1856413 4:854aa2e7eeb2 22 Ticker printTicker;
1856413 0:550f6e86da32 23 //------------------------------------------------------------------------------
1856413 1:2219a519e2bf 24 //Global Variables
1856413 4:854aa2e7eeb2 25
1856413 1:2219a519e2bf 26 double currentPosition1 = 0.0; // Starting position of motor 1 [rad]
1856413 1:2219a519e2bf 27 double currentPosition2 = -(0.5*3.14); // Starting position of motor 2 wrt motor 1 [rad]
1856413 4:854aa2e7eeb2 28 // ^ waarom?
1856413 1:2219a519e2bf 29 int a = 0;
1856413 1:2219a519e2bf 30
1856413 0:550f6e86da32 31 //Inverse Kinematics
1856413 0:550f6e86da32 32 const double L0 = 0.1; // Horizontal length from base to first joint [m] NAMETEN!!
1856413 0:550f6e86da32 33 const double L1 = 0.3; // Length link 1 [m] NAMETEN!!
1856413 0:550f6e86da32 34 const double L2 = 0.3; // Length link 2 [m] NAMETEN!!
1856413 0:550f6e86da32 35 const double L3 = 0.1; // Vertical length from base to first joint [m] NAMETEN!
1856413 0:550f6e86da32 36 volatile double q1 = 0.0; // Starting reference angle of first link [rad]
1856413 0:550f6e86da32 37 volatile double q2 = -(0.5*3.14); // Starting reference angle of second link wrt first link [rad]
1856413 4:854aa2e7eeb2 38 // ^ start buiten bereik, waarom?
1856413 0:550f6e86da32 39 double t = 2.0; // Time interval [s] SHOULD BE REPLACED SAMPLING TIJD!!
1856413 1:2219a519e2bf 40 volatile double vx_des = 0.0; // Starting velocity in x-direction [rad/s]
1856413 1:2219a519e2bf 41 volatile double vy_des = 0.0; // Starting velocity in y-direction [rad/s]
1856413 0:550f6e86da32 42 Matrix Q_set(2,1); // Setting a matrix for storing the angular velocities [rad/sec]
1856413 0:550f6e86da32 43 Matrix J(2,2); // Setting a matrix for the Jacobian
1856413 0:550f6e86da32 44 Matrix V(2,1); // Setting a matrix for storing the EMG-measured velocities
1856413 4:854aa2e7eeb2 45 volatile double errorIK1 = 0.0;
1856413 4:854aa2e7eeb2 46 volatile double errorIK2 = 0.0;
1856413 1:2219a519e2bf 47
1856413 0:550f6e86da32 48 // Motor Control
1856413 0:550f6e86da32 49 volatile double potMeterPosition1 = 0.0;
1856413 2:638c6155d0af 50 //volatile double potMeterPosition2 = 0.0;
1856413 0:550f6e86da32 51 volatile double motorValue1 = 0.01;
1856413 0:550f6e86da32 52 volatile double motorValue2 = 0.01;
1856413 0:550f6e86da32 53 volatile double Kp = 0.34; //dit maken we variabel, dit zorgt voor een grote of kleine overshoot
1856413 0:550f6e86da32 54 volatile double Ki = 0.0; //dit moeten we bepalen met een plot bijvoorbeeld
1856413 0:550f6e86da32 55 volatile double Kd = 0.0;
1856413 0:550f6e86da32 56 volatile double Ts = 0.01; // SAMPLING TIJD!!
1856413 1:2219a519e2bf 57
1856413 0:550f6e86da32 58
1856413 0:550f6e86da32 59 //------------------------------------------------------------------------------
1856413 0:550f6e86da32 60 // Potmeter values TO DETERMINE VX_DES, VY_DES
1856413 2:638c6155d0af 61 void GetPotMeterVelocity1() //Potmeter standard to control X-DIRECTION
1856413 0:550f6e86da32 62 {
1856413 0:550f6e86da32 63 double potMeter1In = potMeter1.read();
1856413 2:638c6155d0af 64 //vx_des = 0.5*3.14*potMeter1In - 0.25*3.14 ; // Reference value y, scaled to -0.25 to 0.25 revolutions
1856413 2:638c6155d0af 65 vx_des = 3.7;
1856413 2:638c6155d0af 66 //return vx_des;
1856413 1:2219a519e2bf 67 }
1856413 1:2219a519e2bf 68
1856413 4:854aa2e7eeb2 69 double SwitchPotmeterVelocity1() //Button has been pressed,
1856413 4:854aa2e7eeb2 70 {
1856413 1:2219a519e2bf 71 vy_des = vx_des;
1856413 1:2219a519e2bf 72 return vy_des;
1856413 1:2219a519e2bf 73 }
1856413 1:2219a519e2bf 74
1856413 1:2219a519e2bf 75 //------------------------------------------------------------------------------
1856413 1:2219a519e2bf 76 // KINEMATIC FUNCTIONS
1856413 1:2219a519e2bf 77
1856413 1:2219a519e2bf 78 Matrix ComputeJ(void)
1856413 1:2219a519e2bf 79 {
1856413 1:2219a519e2bf 80 double a = -sin(q2)/(L1*cos(q1)*sin(q2)-L1*cos(q2)*sin(q1));
1856413 1:2219a519e2bf 81 double b = cos(q2)/(L1*cos(q1)*sin(q2)-L1*cos(q2)*sin(q1));
1856413 1:2219a519e2bf 82 double c = (L1*sin(q1)+L2*sin(q2))/(L1*L2*cos(q1)*sin(q2)-L1*L2*cos(q2)*sin(q1));
1856413 1:2219a519e2bf 83 double d = -(L1*cos(q1)+L2*cos(q2))/(L1*L2*cos(q1)*sin(q2)-L1*L2*cos(q2)*sin(q1));
1856413 1:2219a519e2bf 84 J << a << b
1856413 1:2219a519e2bf 85 << c << d;
1856413 1:2219a519e2bf 86 return J;
1856413 1:2219a519e2bf 87 }
1856413 1:2219a519e2bf 88
1856413 1:2219a519e2bf 89 Matrix ComputeV(void)
1856413 1:2219a519e2bf 90 {
1856413 1:2219a519e2bf 91 V.add(1,1,vx_des); // Add desired x-velocity in V
1856413 1:2219a519e2bf 92 V.add(2,1,vy_des); // Add desired y-velocity in V
1856413 1:2219a519e2bf 93 return V;
1856413 1:2219a519e2bf 94 }
1856413 1:2219a519e2bf 95
1856413 1:2219a519e2bf 96 double IntegrateQ1()
1856413 1:2219a519e2bf 97 {
1856413 1:2219a519e2bf 98 q1 = q1 + (Q_set(1,1))*t; // new value for q1
1856413 1:2219a519e2bf 99 return q1;
1856413 1:2219a519e2bf 100 }
1856413 1:2219a519e2bf 101
1856413 1:2219a519e2bf 102 double IntegrateQ2()
1856413 1:2219a519e2bf 103 {
1856413 1:2219a519e2bf 104 q2 = q2 + (Q_set(2,1))*t; // new value for q2
1856413 1:2219a519e2bf 105 return q2;
1856413 1:2219a519e2bf 106 }
1856413 1:2219a519e2bf 107
1856413 1:2219a519e2bf 108 Matrix ComputeQ_set(void)
1856413 1:2219a519e2bf 109 {
1856413 1:2219a519e2bf 110 float a = J(1,1);
1856413 1:2219a519e2bf 111 float b = J(1,2);
1856413 1:2219a519e2bf 112 float c = J(2,1);
1856413 1:2219a519e2bf 113 float d = J(2,2);
1856413 1:2219a519e2bf 114 float e = V(1,1);
1856413 1:2219a519e2bf 115 float f = V(2,1);
1856413 1:2219a519e2bf 116 float first_row = a*e + b*f;
1856413 1:2219a519e2bf 117 float second_row = c*e + d*f;
1856413 1:2219a519e2bf 118 Q_set.add(1,1,first_row);
1856413 1:2219a519e2bf 119 Q_set.add(2,1,second_row);
1856413 1:2219a519e2bf 120 return Q_set;
1856413 1:2219a519e2bf 121 }
1856413 1:2219a519e2bf 122
1856413 1:2219a519e2bf 123 double ErrorInverseKinematics1()
1856413 1:2219a519e2bf 124 {
1856413 1:2219a519e2bf 125 double errorIK1 = q1 - currentPosition1;
1856413 1:2219a519e2bf 126 return errorIK1;
1856413 0:550f6e86da32 127 }
1856413 4:854aa2e7eeb2 128
1856413 1:2219a519e2bf 129 double ErrorInverseKinematics2()
1856413 0:550f6e86da32 130 {
1856413 1:2219a519e2bf 131 double errorIK2 = q2 - currentPosition2;
1856413 1:2219a519e2bf 132 return errorIK2;
1856413 1:2219a519e2bf 133 }
1856413 1:2219a519e2bf 134 //------------------------------------------------------------------------------
1856413 1:2219a519e2bf 135 // MOTOR PART
1856413 1:2219a519e2bf 136 //Encoder Posities
1856413 1:2219a519e2bf 137 double MeasureEncoderPosition1() // Read current position of motor 1, returns value in [rad]
1856413 1:2219a519e2bf 138 {
1856413 1:2219a519e2bf 139 int counts1i = Encoder1.getPulses();
1856413 1:2219a519e2bf 140 double counts1 = counts1i*1.0f;
1856413 1:2219a519e2bf 141 double measuredPosition1 = (counts1/8400)*6.28; //Rotational position in radians
1856413 1:2219a519e2bf 142 return measuredPosition1;
1856413 0:550f6e86da32 143 }
1856413 0:550f6e86da32 144
1856413 1:2219a519e2bf 145 double MeasureEncoderPosition2() // Read current postion of motor 2, returns value in [rad]
1856413 1:2219a519e2bf 146 {
1856413 1:2219a519e2bf 147 int counts2i = Encoder2.getPulses();
1856413 1:2219a519e2bf 148 double counts2 = counts2i*1.0f;
1856413 1:2219a519e2bf 149 double measuredPosition2 = (counts2/8400)*6.28; //Rotational position in radians
1856413 1:2219a519e2bf 150 return measuredPosition2;
1856413 1:2219a519e2bf 151 }
1856413 1:2219a519e2bf 152
1856413 1:2219a519e2bf 153 double FeedbackControl1(double Error1)
1856413 0:550f6e86da32 154 {
1856413 0:550f6e86da32 155 static double Error_integral1 = 0;
1856413 0:550f6e86da32 156 static double Error_prev1 = Error1;
1856413 0:550f6e86da32 157 // Proportional part:
1856413 0:550f6e86da32 158 double u_k1 = Kp * Error1;
1856413 0:550f6e86da32 159 // Integral part:
1856413 0:550f6e86da32 160 Error_integral1 = Error_integral1 + Error1 * Ts;
1856413 0:550f6e86da32 161 double u_i1 = Ki * Error_integral1;
1856413 1:2219a519e2bf 162 // Derivative part:
1856413 0:550f6e86da32 163 double Error_derivative1 = (Error1 - Error_prev1)/Ts;
1856413 0:550f6e86da32 164 double u_d1 = Kd * Error_derivative1;
1856413 0:550f6e86da32 165 Error_prev1 = Error1;
1856413 0:550f6e86da32 166 // Sum all parts and return it
1856413 0:550f6e86da32 167 return u_k1 + u_i1 + u_d1; //motorValue
1856413 0:550f6e86da32 168 }
1856413 1:2219a519e2bf 169
1856413 0:550f6e86da32 170 double FeedbackControl2(double Error2)
1856413 0:550f6e86da32 171 {
1856413 0:550f6e86da32 172 static double Error_integral2 = 0;
1856413 0:550f6e86da32 173 static double Error_prev2 = Error2;
1856413 0:550f6e86da32 174 // Proportional part:
1856413 0:550f6e86da32 175 double u_k2 = Kp * Error2;
1856413 0:550f6e86da32 176 // Integral part:
1856413 0:550f6e86da32 177 Error_integral2 = Error_integral2 + Error2 * Ts;
1856413 0:550f6e86da32 178 double u_i2 = Ki * Error_integral2;
1856413 1:2219a519e2bf 179 // Derivative part:
1856413 0:550f6e86da32 180 double Error_derivative2 = (Error2 - Error_prev2)/Ts;
1856413 0:550f6e86da32 181 double u_d2 = Kd * Error_derivative2;
1856413 0:550f6e86da32 182 Error_prev2 = Error2;
1856413 0:550f6e86da32 183 // Sum all parts and return it
1856413 0:550f6e86da32 184 return u_k2 + u_i2 + u_d2; //motorValue
1856413 0:550f6e86da32 185 }
1856413 1:2219a519e2bf 186
1856413 0:550f6e86da32 187 void SetMotor1(double motorValue1)
1856413 0:550f6e86da32 188 {
1856413 0:550f6e86da32 189 // Given -1<=motorValue<=1, this sets the PWM and direction
1856413 0:550f6e86da32 190 // bits for motor 1. Positive value makes motor rotating
1856413 0:550f6e86da32 191 // clockwise. motorValues outside range are truncated to
1856413 0:550f6e86da32 192 // within range
1856413 0:550f6e86da32 193 // 0 = clockwise motor direction
1856413 0:550f6e86da32 194 // 1 = counterclockwise motor direction
1856413 0:550f6e86da32 195 if (motorValue1 >=0) {
1856413 0:550f6e86da32 196 motor1Direction=0;
1856413 0:550f6e86da32 197 } else {
1856413 0:550f6e86da32 198 motor1Direction=1;
1856413 0:550f6e86da32 199 }
1856413 0:550f6e86da32 200 if (fabs(motorValue1)>1) {
1856413 0:550f6e86da32 201 motor1PWM = 1;
1856413 0:550f6e86da32 202 } else {
1856413 0:550f6e86da32 203 motor1PWM = fabs(motorValue1);
1856413 0:550f6e86da32 204 }
1856413 0:550f6e86da32 205 }
1856413 1:2219a519e2bf 206
1856413 0:550f6e86da32 207 void SetMotor2(double motorValue2)
1856413 0:550f6e86da32 208 {
1856413 0:550f6e86da32 209 // Given -1<=motorValue<=1, this sets the PWM and direction
1856413 0:550f6e86da32 210 // bits for motor 1. Positive value makes motor rotating
1856413 0:550f6e86da32 211 // clockwise. motorValues outside range are truncated to
1856413 0:550f6e86da32 212 // within range
1856413 0:550f6e86da32 213 // 0 = counterclockwise motor direction
1856413 0:550f6e86da32 214 // 1 = clockwise motor direction
1856413 0:550f6e86da32 215 if (motorValue2 >=0) {
1856413 0:550f6e86da32 216 motor2Direction=1;
1856413 0:550f6e86da32 217 } else {
1856413 0:550f6e86da32 218 motor2Direction=0;
1856413 0:550f6e86da32 219 }
1856413 0:550f6e86da32 220 if (fabs(motorValue2)>1) {
1856413 0:550f6e86da32 221 motor2PWM = 1;
1856413 0:550f6e86da32 222 } else {
1856413 0:550f6e86da32 223 motor2PWM = fabs(motorValue2);
1856413 0:550f6e86da32 224 }
1856413 0:550f6e86da32 225 }
1856413 1:2219a519e2bf 226
1856413 1:2219a519e2bf 227 /*void MeasureAndControl1(void)
1856413 0:550f6e86da32 228 {
1856413 0:550f6e86da32 229 // This function determines the desired velocity, measures the
1856413 0:550f6e86da32 230 // actual velocity, and controls the motor with
1856413 0:550f6e86da32 231 // a simple Feedback controller. Call this from a Ticker.
1856413 2:638c6155d0af 232 MeterPosition1 = q1;
1856413 0:550f6e86da32 233 currentPosition1 = MeasureEncoderPosition1();
1856413 0:550f6e86da32 234 motorValue1 = FeedbackControl1(potMeterPosition1 - currentPosition1);
1856413 0:550f6e86da32 235 SetMotor1(motorValue1);
1856413 0:550f6e86da32 236 }
1856413 1:2219a519e2bf 237
1856413 0:550f6e86da32 238 void MeasureAndControl2(void)
1856413 0:550f6e86da32 239 {
1856413 0:550f6e86da32 240 // This function determines the desired velocity, measures the
1856413 0:550f6e86da32 241 // actual velocity, and controls the motor with
1856413 0:550f6e86da32 242 // a simple Feedback controller. Call this from a Ticker.
1856413 0:550f6e86da32 243 potMeterPosition2 = q2;
1856413 0:550f6e86da32 244 currentPosition2 = MeasureEncoderPosition2();
1856413 0:550f6e86da32 245 motorValue2 = FeedbackControl2(potMeterPosition2 - currentPosition2);
1856413 0:550f6e86da32 246 SetMotor2(motorValue2);
1856413 1:2219a519e2bf 247 }*/
1856413 4:854aa2e7eeb2 248 void AllesinEen() //HIER GAAT IETS MIS!!
1856413 4:854aa2e7eeb2 249 {
1856413 4:854aa2e7eeb2 250 currentPosition1 = MeasureEncoderPosition1(); // You want to know the current angle of the motors to get the right Jacobian
1856413 4:854aa2e7eeb2 251 currentPosition2 = MeasureEncoderPosition2(); // You want to know the current angle of the motors to get the right Jacobian
1856413 4:854aa2e7eeb2 252 J = ComputeJ(); // Compute matrix J (inverse Jacobian, and Twist is already left out)
1856413 4:854aa2e7eeb2 253 V = ComputeV(); // Compute matrix V (stores the desired velocities obtained from the EMG signal)
1856413 4:854aa2e7eeb2 254 Q_set = ComputeQ_set(); // Compute Q_set (stores Q1 and Q2)
1856413 4:854aa2e7eeb2 255 q1 = IntegrateQ1(); // Compute required angle to go to desired position of end-effector
1856413 4:854aa2e7eeb2 256 q2 = IntegrateQ2(); // Compute required angle to go to desired position of end-effector
1856413 4:854aa2e7eeb2 257 if (q1 < 1.047) { // q1 can only be smaller than 1.047 rad
1856413 4:854aa2e7eeb2 258 q1 = q1;
1856413 4:854aa2e7eeb2 259 } else { // If value of q1 is greater than 1.047 rad, then stay at maximum angle
1856413 4:854aa2e7eeb2 260 q1 = 1.047;
1856413 4:854aa2e7eeb2 261 }
1856413 4:854aa2e7eeb2 262 if (q2 < 0.61) { // q2 cannot be smaller than 0.61 rad
1856413 4:854aa2e7eeb2 263 q2 = 0.61; // Stay at mimimum angle
1856413 4:854aa2e7eeb2 264 } else if (q2 > 1.57) { // q2 cannot be greater than 1.57 rad
1856413 4:854aa2e7eeb2 265 q2 = 1.57; // Stay at maximum angle
1856413 4:854aa2e7eeb2 266 } else { // If q2 is in the right range, let calculated q2 be q2
1856413 4:854aa2e7eeb2 267 q2 = q2;
1856413 4:854aa2e7eeb2 268 }
1856413 4:854aa2e7eeb2 269 // Determine error and add PID control
1856413 4:854aa2e7eeb2 270 errorIK1 = ErrorInverseKinematics1(); // Determine difference between current angle motor 1 and desired angle
1856413 4:854aa2e7eeb2 271 errorIK2 = ErrorInverseKinematics2(); // Determine difference between current angle motor 2 and desired angle
1856413 4:854aa2e7eeb2 272 // Determine motorValues
1856413 4:854aa2e7eeb2 273 motorValue1 = FeedbackControl1(errorIK1); // Calculate motorValue1
1856413 4:854aa2e7eeb2 274 motorValue2 = FeedbackControl2(errorIK2); // Calculate motorValue2
1856413 4:854aa2e7eeb2 275 // Make Motor move
1856413 4:854aa2e7eeb2 276 SetMotor1(motorValue1);
1856413 4:854aa2e7eeb2 277 SetMotor2(motorValue2);
1856413 0:550f6e86da32 278
1856413 4:854aa2e7eeb2 279 // Press button to switch velocity direction
1856413 4:854aa2e7eeb2 280 //if (!button2.read()) {
1856413 4:854aa2e7eeb2 281 if (!button2.read()) { //button2 blijven indrukken
1856413 4:854aa2e7eeb2 282 //SwitchPotmeterVelocity1();
1856413 4:854aa2e7eeb2 283 vy_des=vx_des;
1856413 4:854aa2e7eeb2 284 vx_des=0;
1856413 4:854aa2e7eeb2 285 } else {
1856413 4:854aa2e7eeb2 286 vy_des=0;
1856413 4:854aa2e7eeb2 287 }
1856413 4:854aa2e7eeb2 288 }
1856413 4:854aa2e7eeb2 289
1856413 4:854aa2e7eeb2 290 void printen()
1856413 4:854aa2e7eeb2 291 {
1856413 4:854aa2e7eeb2 292 pc.printf("vx_des = %f \t vy_des = %f \r\n", vx_des, vy_des);
1856413 4:854aa2e7eeb2 293 pc.printf("Error IK1 = %f \t Error IK2 = %f \r\n", errorIK1, errorIK2);
1856413 4:854aa2e7eeb2 294 pc.printf("MotorValue 1 = %f \t MotorValue 2 = %f \r\n", motorValue1, motorValue2);
1856413 4:854aa2e7eeb2 295 pc.printf("q1 Qset = %f \t q2 Qset = %f \r\n", Q_set(1,1), Q_set(2,1));
1856413 4:854aa2e7eeb2 296 }
1856413 1:2219a519e2bf 297 //------------------------------------------------------------------------------
1856413 1:2219a519e2bf 298 // MAIN
1856413 1:2219a519e2bf 299 int main()
1856413 1:2219a519e2bf 300 {
1856413 0:550f6e86da32 301 pc.baud(115200);
1856413 4:854aa2e7eeb2 302 potmeterTicker.attach(GetPotMeterVelocity1, 0.01f);
1856413 4:854aa2e7eeb2 303 allesineenticker.attach(AllesinEen, 0.01f);
1856413 4:854aa2e7eeb2 304 printTicker.attach(printen, 0.5f);
1856413 4:854aa2e7eeb2 305
1856413 1:2219a519e2bf 306 while (true) {
1856413 0:550f6e86da32 307
1856413 4:854aa2e7eeb2 308 //wait(0.01);
1856413 0:550f6e86da32 309 }
1856413 0:550f6e86da32 310 }