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?
User | Revision | Line number | New 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 | } |