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@0:550f6e86da32, 2018-11-01 (annotated)

Committer:
1856413
Date:
Thu Nov 01 16:52:17 2018 +0000
Revision:
0:550f6e86da32
Child:
1:2219a519e2bf
Demo State with IK and MotorControl and potmeters to determine vx _des and vy_des

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 0:550f6e86da32 8 DigitalOut motor1DirectionPin(D7);
1856413 0:550f6e86da32 9 DigitalOut motor2DirectionPin(D4);
1856413 0:550f6e86da32 10 DigitalOut Led(LED_GREEN);
1856413 0:550f6e86da32 11 DigitalIn button2(SW3);
1856413 0:550f6e86da32 12 FastPWM motor1MagnitudePin(D6);
1856413 0:550f6e86da32 13 FastPWM motor2MagnitudePin(D5);
1856413 0:550f6e86da32 14 AnalogIn potMeter1(A4);
1856413 0:550f6e86da32 15 AnalogIn potMeter2(A5);
1856413 0:550f6e86da32 16 InterruptIn button2(D3);
1856413 0:550f6e86da32 17 QEI Encoder1 (D12, D13, NC, 64, QEI::X4_ENCODING);
1856413 0:550f6e86da32 18 QEI Encoder2 (D10, D11, NC, 64, QEI::X4_ENCODING);
1856413 0:550f6e86da32 19
1856413 0:550f6e86da32 20 // Tickers
1856413 0:550f6e86da32 21 Ticker startMotor;
1856413 0:550f6e86da32 22 Ticker printTicker;
1856413 0:550f6e86da32 23
1856413 0:550f6e86da32 24 //------------------------------------------------------------------------------
1856413 0:550f6e86da32 25 //Inverse Kinematics
1856413 0:550f6e86da32 26 const double L0 = 0.1; // Horizontal length from base to first joint [m] NAMETEN!!
1856413 0:550f6e86da32 27 const double L1 = 0.3; // Length link 1 [m] NAMETEN!!
1856413 0:550f6e86da32 28 const double L2 = 0.3; // Length link 2 [m] NAMETEN!!
1856413 0:550f6e86da32 29 const double L3 = 0.1; // Vertical length from base to first joint [m] NAMETEN!
1856413 0:550f6e86da32 30 volatile double q1 = 0.0; // Starting reference angle of first link [rad]
1856413 0:550f6e86da32 31 volatile double q2 = -(0.5*3.14); // Starting reference angle of second link wrt first link [rad]
1856413 0:550f6e86da32 32 double t = 2.0; // Time interval [s] SHOULD BE REPLACED SAMPLING TIJD!!
1856413 0:550f6e86da32 33 Matrix Q_set(2,1); // Setting a matrix for storing the angular velocities [rad/sec]
1856413 0:550f6e86da32 34 Matrix J(2,2); // Setting a matrix for the Jacobian
1856413 0:550f6e86da32 35 Matrix V(2,1); // Setting a matrix for storing the EMG-measured velocities
1856413 0:550f6e86da32 36
1856413 0:550f6e86da32 37 // Motor Control
1856413 0:550f6e86da32 38 volatile double potMeterPosition1 = 0.0;
1856413 0:550f6e86da32 39 volatile double potMeterPosition2 = 0.0;
1856413 0:550f6e86da32 40 volatile double motorValue1 = 0.01;
1856413 0:550f6e86da32 41 volatile double motorValue2 = 0.01;
1856413 0:550f6e86da32 42 volatile double Kp = 0.34; //dit maken we variabel, dit zorgt voor een grote of kleine overshoot
1856413 0:550f6e86da32 43 volatile double Ki = 0.0; //dit moeten we bepalen met een plot bijvoorbeeld
1856413 0:550f6e86da32 44 volatile double Kd = 0.0;
1856413 0:550f6e86da32 45 volatile double Ts = 0.01; // SAMPLING TIJD!!
1856413 0:550f6e86da32 46
1856413 0:550f6e86da32 47
1856413 0:550f6e86da32 48 //------------------------------------------------------------------------------
1856413 0:550f6e86da32 49 // Potmeter values TO DETERMINE VX_DES, VY_DES
1856413 0:550f6e86da32 50 double GetPotMeterPosition1() // Measure the current Potmeter1 value
1856413 0:550f6e86da32 51 {
1856413 0:550f6e86da32 52 double potMeter1In = potMeter1.read();
1856413 0:550f6e86da32 53 potMeterPosition1 = 8.0*3.14*potMeter1In - 4.0*3.14 ; // Reference value y, scaled to -4 to 4 revolutions
1856413 0:550f6e86da32 54 return potMeterPosition1;
1856413 0:550f6e86da32 55 }
1856413 0:550f6e86da32 56
1856413 0:550f6e86da32 57 double GetPotMeterPosition2() // Measure the current Potmeter2 value
1856413 0:550f6e86da32 58 {
1856413 0:550f6e86da32 59 double potMeter2In = potMeter2.read();
1856413 0:550f6e86da32 60 potMeterPosition2 = 8.0*3.14*potMeter2In - 4.0*3.14 ;
1856413 0:550f6e86da32 61 return potMeterPosition2;
1856413 0:550f6e86da32 62 }
1856413 0:550f6e86da32 63
1856413 0:550f6e86da32 64 // Drive the motor
1856413 0:550f6e86da32 65 double FeedbackControl1(double Error1) // Dit moet zo geschreven worden dat het zowel met EMG als met potmeters gebruikt kan worden
1856413 0:550f6e86da32 66 {
1856413 0:550f6e86da32 67 static double Error_integral1 = 0;
1856413 0:550f6e86da32 68 static double Error_prev1 = Error1;
1856413 0:550f6e86da32 69 //static BiQuad LowPassFilter(..., ..., ..., ..., ...)
1856413 0:550f6e86da32 70 // Proportional part:
1856413 0:550f6e86da32 71 //van 0 tot 20, waardes rond de 5 zijn het beste (minder overshoot + minder trilling motor beste combinatie hiervan)
1856413 0:550f6e86da32 72 double u_k1 = Kp * Error1;
1856413 0:550f6e86da32 73 // Integral part:
1856413 0:550f6e86da32 74 Error_integral1 = Error_integral1 + Error1 * Ts;
1856413 0:550f6e86da32 75 double u_i1 = Ki * Error_integral1;
1856413 0:550f6e86da32 76 // Derivative part
1856413 0:550f6e86da32 77 double Error_derivative1 = (Error1 - Error_prev1)/Ts;
1856413 0:550f6e86da32 78 double u_d1 = Kd * Error_derivative1;
1856413 0:550f6e86da32 79 Error_prev1 = Error1;
1856413 0:550f6e86da32 80 // Sum all parts and return it
1856413 0:550f6e86da32 81 return u_k1 + u_i1 + u_d1; //motorValue
1856413 0:550f6e86da32 82 }
1856413 0:550f6e86da32 83
1856413 0:550f6e86da32 84 double FeedbackControl2(double Error2)
1856413 0:550f6e86da32 85 {
1856413 0:550f6e86da32 86 static double Error_integral2 = 0;
1856413 0:550f6e86da32 87 static double Error_prev2 = Error2;
1856413 0:550f6e86da32 88 //static BiQuad LowPassFilter(..., ..., ..., ..., ...)
1856413 0:550f6e86da32 89 // Proportional part:
1856413 0:550f6e86da32 90 //van 0 tot 20, waardes rond de 5 zijn het beste (minder overshoot + minder trilling motor beste combinatie hiervan)
1856413 0:550f6e86da32 91 double u_k2 = Kp * Error2;
1856413 0:550f6e86da32 92 // Integral part:
1856413 0:550f6e86da32 93 Error_integral2 = Error_integral2 + Error2 * Ts;
1856413 0:550f6e86da32 94 double u_i2 = Ki * Error_integral2;
1856413 0:550f6e86da32 95 // Derivative part
1856413 0:550f6e86da32 96 double Error_derivative2 = (Error2 - Error_prev2)/Ts;
1856413 0:550f6e86da32 97 double u_d2 = Kd * Error_derivative2;
1856413 0:550f6e86da32 98 Error_prev2 = Error2;
1856413 0:550f6e86da32 99 // Sum all parts and return it
1856413 0:550f6e86da32 100 return u_k2 + u_i2 + u_d2; //motorValue
1856413 0:550f6e86da32 101 }
1856413 0:550f6e86da32 102
1856413 0:550f6e86da32 103 void SetMotor1(double motorValue1)
1856413 0:550f6e86da32 104 {
1856413 0:550f6e86da32 105 // Given -1<=motorValue<=1, this sets the PWM and direction
1856413 0:550f6e86da32 106 // bits for motor 1. Positive value makes motor rotating
1856413 0:550f6e86da32 107 // clockwise. motorValues outside range are truncated to
1856413 0:550f6e86da32 108 // within range
1856413 0:550f6e86da32 109 // 0 = clockwise motor direction
1856413 0:550f6e86da32 110 // 1 = counterclockwise motor direction
1856413 0:550f6e86da32 111 if (motorValue1 >=0) {
1856413 0:550f6e86da32 112 motor1Direction=0;
1856413 0:550f6e86da32 113 } else {
1856413 0:550f6e86da32 114 motor1Direction=1;
1856413 0:550f6e86da32 115 }
1856413 0:550f6e86da32 116 if (fabs(motorValue1)>1) {
1856413 0:550f6e86da32 117 motor1PWM = 1;
1856413 0:550f6e86da32 118 } else {
1856413 0:550f6e86da32 119 motor1PWM = fabs(motorValue1);
1856413 0:550f6e86da32 120 }
1856413 0:550f6e86da32 121 }
1856413 0:550f6e86da32 122
1856413 0:550f6e86da32 123 void SetMotor2(double motorValue2)
1856413 0:550f6e86da32 124 {
1856413 0:550f6e86da32 125 // Given -1<=motorValue<=1, this sets the PWM and direction
1856413 0:550f6e86da32 126 // bits for motor 1. Positive value makes motor rotating
1856413 0:550f6e86da32 127 // clockwise. motorValues outside range are truncated to
1856413 0:550f6e86da32 128 // within range
1856413 0:550f6e86da32 129 // 0 = counterclockwise motor direction
1856413 0:550f6e86da32 130 // 1 = clockwise motor direction
1856413 0:550f6e86da32 131 if (motorValue2 >=0) {
1856413 0:550f6e86da32 132 motor2Direction=1;
1856413 0:550f6e86da32 133 } else {
1856413 0:550f6e86da32 134 motor2Direction=0;
1856413 0:550f6e86da32 135 }
1856413 0:550f6e86da32 136 if (fabs(motorValue2)>1) {
1856413 0:550f6e86da32 137 motor2PWM = 1;
1856413 0:550f6e86da32 138 } else {
1856413 0:550f6e86da32 139 motor2PWM = fabs(motorValue2);
1856413 0:550f6e86da32 140 }
1856413 0:550f6e86da32 141 }
1856413 0:550f6e86da32 142
1856413 0:550f6e86da32 143 void MeasureAndControl1(void)
1856413 0:550f6e86da32 144 {
1856413 0:550f6e86da32 145 // This function determines the desired velocity, measures the
1856413 0:550f6e86da32 146 // actual velocity, and controls the motor with
1856413 0:550f6e86da32 147 // a simple Feedback controller. Call this from a Ticker.
1856413 0:550f6e86da32 148 potMeterPosition1 = q1;
1856413 0:550f6e86da32 149 currentPosition1 = MeasureEncoderPosition1();
1856413 0:550f6e86da32 150 motorValue1 = FeedbackControl1(potMeterPosition1 - currentPosition1);
1856413 0:550f6e86da32 151 SetMotor1(motorValue1);
1856413 0:550f6e86da32 152 }
1856413 0:550f6e86da32 153
1856413 0:550f6e86da32 154 void MeasureAndControl2(void)
1856413 0:550f6e86da32 155 {
1856413 0:550f6e86da32 156 // This function determines the desired velocity, measures the
1856413 0:550f6e86da32 157 // actual velocity, and controls the motor with
1856413 0:550f6e86da32 158 // a simple Feedback controller. Call this from a Ticker.
1856413 0:550f6e86da32 159 potMeterPosition2 = q2;
1856413 0:550f6e86da32 160 currentPosition2 = MeasureEncoderPosition2();
1856413 0:550f6e86da32 161 motorValue2 = FeedbackControl2(potMeterPosition2 - currentPosition2);
1856413 0:550f6e86da32 162 SetMotor2(motorValue2);
1856413 0:550f6e86da32 163
1856413 0:550f6e86da32 164 //------------------------------------------------------------------------------
1856413 0:550f6e86da32 165 // Kinematic functions
1856413 0:550f6e86da32 166
1856413 0:550f6e86da32 167 Matrix ComputeJ(void){
1856413 0:550f6e86da32 168 double a = -sin(q2)/(L1*cos(q1)*sin(q2)-L1*cos(q2)*sin(q1));
1856413 0:550f6e86da32 169 double b = cos(q2)/(L1*cos(q1)*sin(q2)-L1*cos(q2)*sin(q1));
1856413 0:550f6e86da32 170 double c = (L1*sin(q1)+L2*sin(q2))/(L1*L2*cos(q1)*sin(q2)-L1*L2*cos(q2)*sin(q1));
1856413 0:550f6e86da32 171 double d = -(L1*cos(q1)+L2*cos(q2))/(L1*L2*cos(q1)*sin(q2)-L1*L2*cos(q2)*sin(q1));
1856413 0:550f6e86da32 172 J << a << b
1856413 0:550f6e86da32 173 << c << d;
1856413 0:550f6e86da32 174 return J;
1856413 0:550f6e86da32 175 }
1856413 0:550f6e86da32 176
1856413 0:550f6e86da32 177 Matrix ComputeV(void){
1856413 0:550f6e86da32 178 V.add(1,1,vx_des); // Add desired x-velocity in V
1856413 0:550f6e86da32 179 V.add(2,1,vy_des); // Add desired y-velocity in V
1856413 0:550f6e86da32 180 return V;
1856413 0:550f6e86da32 181 }
1856413 0:550f6e86da32 182
1856413 0:550f6e86da32 183 double IntegrateQ1(){
1856413 0:550f6e86da32 184 q1 = q1 + (Q_set(1,1))*t; // new value for q1
1856413 0:550f6e86da32 185 return q1;
1856413 0:550f6e86da32 186 }
1856413 0:550f6e86da32 187
1856413 0:550f6e86da32 188 double IntegrateQ2(){
1856413 0:550f6e86da32 189 q2 = q2 + (Q_set(2,1))*t; // new value for q2
1856413 0:550f6e86da32 190 return q2;
1856413 0:550f6e86da32 191 }
1856413 0:550f6e86da32 192
1856413 0:550f6e86da32 193 Matrix ComputeQ_set(void){
1856413 0:550f6e86da32 194 float a = J(1,1);
1856413 0:550f6e86da32 195 float b = J(1,2);
1856413 0:550f6e86da32 196 float c = J(2,1);
1856413 0:550f6e86da32 197 float d = J(2,2);
1856413 0:550f6e86da32 198 float e = V(1,1);
1856413 0:550f6e86da32 199 float f = V(2,1);
1856413 0:550f6e86da32 200 float first_row = a*e + b*f;
1856413 0:550f6e86da32 201 float second_row = c*e + d*f;
1856413 0:550f6e86da32 202 Q_set.add(1,1,first_row);
1856413 0:550f6e86da32 203 Q_set.add(2,1,second_row);
1856413 0:550f6e86da32 204 return Q_set;
1856413 0:550f6e86da32 205 }
1856413 0:550f6e86da32 206
1856413 0:550f6e86da32 207 int main(){
1856413 0:550f6e86da32 208 //led = 0;
1856413 0:550f6e86da32 209 pc.baud(115200);
1856413 0:550f6e86da32 210 pc.printf("\r\nDoet het script het?\r\n");
1856413 0:550f6e86da32 211 // Compute Jacobian
1856413 0:550f6e86da32 212 J = ComputeJ();
1856413 0:550f6e86da32 213 J.print();
1856413 0:550f6e86da32 214 pc.printf("\r\n");
1856413 0:550f6e86da32 215 // Compute velocities
1856413 0:550f6e86da32 216 V = ComputeV();
1856413 0:550f6e86da32 217 V.print();
1856413 0:550f6e86da32 218 pc.printf("\r\n");
1856413 0:550f6e86da32 219 // Multiplying matrix J and V and storing in array Q_set
1856413 0:550f6e86da32 220 Q_set = ComputeQ_set();
1856413 0:550f6e86da32 221
1856413 0:550f6e86da32 222 // velocity to position
1856413 0:550f6e86da32 223 q1 = IntegrateQ1();
1856413 0:550f6e86da32 224 q2 = IntegrateQ2();
1856413 0:550f6e86da32 225
1856413 0:550f6e86da32 226 /*if (q1 > ... && < ...) {
1856413 0:550f6e86da32 227 TurnMotorsOff;
1856413 0:550f6e86da32 228 /go to failure mode
1856413 0:550f6e86da32 229 }
1856413 0:550f6e86da32 230 if (q2 > ... && < ...) {
1856413 0:550f6e86da32 231 TurnMotorsOff/go to failure mode
1856413 0:550f6e86da32 232 }*/
1856413 0:550f6e86da32 233
1856413 0:550f6e86da32 234 pc.printf("New position q1: %f, New Position q2: %f", q1, q2);
1856413 0:550f6e86da32 235
1856413 0:550f6e86da32 236 }
1856413 0:550f6e86da32 237
1856413 0:550f6e86da32 238 int main() {
1856413 0:550f6e86da32 239 currentPosition1 = MeasureEncoderPosition1(); // You want to know the current angle of the motors to get the right Jacobian
1856413 0:550f6e86da32 240 currentPosition2 = MeasureEncoderPosition2(); // You want to know the current angle of the motors to get the right Jacobian
1856413 0:550f6e86da32 241 J = ComputeJ(); // Compute matrix J (inverse Jacobian, and Twist is already left out)
1856413 0:550f6e86da32 242 V = ComputeV(); // Compute matrix V (stores the desired velocities obtained from the EMG signal)
1856413 0:550f6e86da32 243 Q_set = ComputeQ_set(); // Compute Q_set (stores Q1 and Q2)
1856413 0:550f6e86da32 244 IntegrateQ1(); // Compute required angle to go to desired position of end-effector
1856413 0:550f6e86da32 245 IntegrateQ2(); // Compute required angle to go to desired position of end-effector
1856413 0:550f6e86da32 246 if (q1 < 1.047) { // q1 can only be smaller than 1.047 rad
1856413 0:550f6e86da32 247 q1 = q1;
1856413 0:550f6e86da32 248 } else { // If value of q1 is greater than 1.047 rad, then stay at maximum angle
1856413 0:550f6e86da32 249 q1 = 1.047;
1856413 0:550f6e86da32 250 }
1856413 0:550f6e86da32 251 if (q2 < 0.61) { // q2 cannot be smaller than 0.61 rad
1856413 0:550f6e86da32 252 q2 = 0.61; // Stay at mimimum angle
1856413 0:550f6e86da32 253 } else if (q2 > 1.57) { // q2 cannot be greater than 1.57 rad
1856413 0:550f6e86da32 254 q2 = 1.57; // Stay at maximum angle
1856413 0:550f6e86da32 255 } else { // If q2 is in the right range, let calculated q2 be q2
1856413 0:550f6e86da32 256 q2 = q2;
1856413 0:550f6e86da32 257 }
1856413 0:550f6e86da32 258 // Determine error and add PID control
1856413 0:550f6e86da32 259 double errorIK1 = ErrorInverseKinematics1(); // Determine difference between current angle motor 1 and desired angle
1856413 0:550f6e86da32 260 double errorIK2 = ErrorInverseKinematics2(); // Determine difference between current angle motor 2 and desired angle
1856413 0:550f6e86da32 261 // Determine motorValues
1856413 0:550f6e86da32 262 motorValue1 = FeedbackControl1(errorIK1); // Calculate motorValue1
1856413 0:550f6e86da32 263 motorValue2 = FeedbackControl2(errorIK2); // Calculate motorValue2
1856413 0:550f6e86da32 264 // Make Motor move
1856413 0:550f6e86da32 265 SetMotor1(motorValue1);
1856413 0:550f6e86da32 266 SetMotor2(motorValue2);
1856413 0:550f6e86da32 267
1856413 0:550f6e86da32 268 while (true){}
1856413 0:550f6e86da32 269 }