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@8:3d2228402c71, 2018-11-06 (annotated)
- Committer:
- 1856413
- Date:
- Tue Nov 06 20:25:40 2018 +0000
- Revision:
- 8:3d2228402c71
- Parent:
- 7:fb3da4df4269
- Child:
- 9:7443c37f0f7b
Final demo script
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 | 8:3d2228402c71 | 11 | DigitalIn button2(SW3); |
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 | 8:3d2228402c71 | 21 | Ticker measurecontrolTicker; |
1856413 | 4:854aa2e7eeb2 | 22 | Ticker printTicker; |
1856413 | 0:550f6e86da32 | 23 | //------------------------------------------------------------------------------ |
1856413 | 1:2219a519e2bf | 24 | //Global Variables |
1856413 | 4:854aa2e7eeb2 | 25 | |
1856413 | 8:3d2228402c71 | 26 | volatile float currentPosition1 = 0.1; // Starting position of motor 1 [rad] |
1856413 | 8:3d2228402c71 | 27 | volatile float currentPosition2 = -(0.5*3.14); // Starting position of motor 2 wrt motor 1 [rad] |
1856413 | 1:2219a519e2bf | 28 | |
1856413 | 0:550f6e86da32 | 29 | //Inverse Kinematics |
1856413 | 8:3d2228402c71 | 30 | const float L0 = 0.1; // Horizontal length from base to first joint [m] |
1856413 | 8:3d2228402c71 | 31 | const float L1 = 0.3; // Length link 1 [m] |
1856413 | 8:3d2228402c71 | 32 | const float L2 = 0.3; // Length link 2 [m] |
1856413 | 8:3d2228402c71 | 33 | const float L3 = 0.1; // Vertical length from base to first joint [m] |
nicollevanrijswijk | 5:aca2af310419 | 34 | volatile float q1 = 0.1; // Starting reference angle of first link [rad] |
nicollevanrijswijk | 5:aca2af310419 | 35 | volatile float q2 = -(0.5*3.12); // Starting reference angle of second link wrt first link [rad] |
1856413 | 8:3d2228402c71 | 36 | |
1856413 | 8:3d2228402c71 | 37 | float t = 0.02; // Time interval [s] |
nicollevanrijswijk | 5:aca2af310419 | 38 | volatile float vx_des = 0.0; // Starting velocity in x-direction [rad/s] |
nicollevanrijswijk | 5:aca2af310419 | 39 | volatile float vy_des = 0.0; // Starting velocity in y-direction [rad/s] |
nicollevanrijswijk | 5:aca2af310419 | 40 | volatile float errorIK1 = 0.0; |
nicollevanrijswijk | 5:aca2af310419 | 41 | volatile float errorIK2 = 0.0; |
nicollevanrijswijk | 5:aca2af310419 | 42 | volatile float J1 = 0.0; |
nicollevanrijswijk | 5:aca2af310419 | 43 | volatile float J2 = 0.0; |
nicollevanrijswijk | 5:aca2af310419 | 44 | volatile float J3 = 0.0; |
nicollevanrijswijk | 5:aca2af310419 | 45 | volatile float J4 = 0.0; |
nicollevanrijswijk | 5:aca2af310419 | 46 | volatile float V1 = 0.0; |
nicollevanrijswijk | 5:aca2af310419 | 47 | volatile float V2 = 0.0; |
nicollevanrijswijk | 5:aca2af310419 | 48 | volatile float Q1 = 0.0; |
nicollevanrijswijk | 5:aca2af310419 | 49 | volatile float Q2 = 0.0; |
1856413 | 1:2219a519e2bf | 50 | |
1856413 | 0:550f6e86da32 | 51 | // Motor Control |
nicollevanrijswijk | 5:aca2af310419 | 52 | volatile float potMeterPosition1 = 0.0; |
nicollevanrijswijk | 5:aca2af310419 | 53 | int counts1i = 0; |
nicollevanrijswijk | 5:aca2af310419 | 54 | int counts2i = 0; |
nicollevanrijswijk | 5:aca2af310419 | 55 | //volatile float potMeterPosition2 = 0.0; |
nicollevanrijswijk | 5:aca2af310419 | 56 | volatile float motorValue1 = 0.01; |
nicollevanrijswijk | 5:aca2af310419 | 57 | volatile float motorValue2 = 0.01; |
1856413 | 8:3d2228402c71 | 58 | volatile float Kp = 0.34; |
1856413 | 8:3d2228402c71 | 59 | volatile float Ki = 0.0; |
nicollevanrijswijk | 5:aca2af310419 | 60 | volatile float Kd = 0.0; |
1856413 | 8:3d2228402c71 | 61 | volatile float Ts = 0.02; |
1856413 | 1:2219a519e2bf | 62 | |
1856413 | 0:550f6e86da32 | 63 | |
1856413 | 0:550f6e86da32 | 64 | //------------------------------------------------------------------------------ |
1856413 | 0:550f6e86da32 | 65 | // Potmeter values TO DETERMINE VX_DES, VY_DES |
1856413 | 2:638c6155d0af | 66 | void GetPotMeterVelocity1() //Potmeter standard to control X-DIRECTION |
1856413 | 0:550f6e86da32 | 67 | { |
nicollevanrijswijk | 5:aca2af310419 | 68 | float potMeter1In = potMeter1.read(); |
1856413 | 8:3d2228402c71 | 69 | vx_des = 4.0*3.14*potMeter1In - 2.0*3.14; |
1856413 | 1:2219a519e2bf | 70 | } |
1856413 | 1:2219a519e2bf | 71 | |
nicollevanrijswijk | 5:aca2af310419 | 72 | float SwitchPotmeterVelocity1() //Button has been pressed, |
1856413 | 4:854aa2e7eeb2 | 73 | { |
1856413 | 1:2219a519e2bf | 74 | vy_des = vx_des; |
1856413 | 1:2219a519e2bf | 75 | return vy_des; |
1856413 | 1:2219a519e2bf | 76 | } |
1856413 | 1:2219a519e2bf | 77 | |
1856413 | 1:2219a519e2bf | 78 | //------------------------------------------------------------------------------ |
1856413 | 1:2219a519e2bf | 79 | // KINEMATIC FUNCTIONS |
1856413 | 1:2219a519e2bf | 80 | |
nicollevanrijswijk | 5:aca2af310419 | 81 | void ComputeJ(void) |
1856413 | 1:2219a519e2bf | 82 | { |
nicollevanrijswijk | 7:fb3da4df4269 | 83 | J1 = -sin(q1)/(L1*cos(q1)*sin(q2)-L1*cos(q2)*sin(q1)); |
nicollevanrijswijk | 7:fb3da4df4269 | 84 | J2 = cos(q2)/(L1*cos(q1)*sin(q2)-L1*cos(q2)*sin(q1)); |
nicollevanrijswijk | 7:fb3da4df4269 | 85 | J3 = (L1*sin(q1)+L2*sin(q2))/(L1*L2*cos(q1)*sin(q2)-L1*L2*cos(q2)*sin(q1)); |
nicollevanrijswijk | 7:fb3da4df4269 | 86 | J4 = -(L1*cos(q1)+L2*cos(q2))/(L1*L2*cos(q1)*sin(q2)-L1*L2*cos(q2)*sin(q1)); |
1856413 | 1:2219a519e2bf | 87 | } |
1856413 | 1:2219a519e2bf | 88 | |
nicollevanrijswijk | 5:aca2af310419 | 89 | void ComputeV(void) |
1856413 | 1:2219a519e2bf | 90 | { |
nicollevanrijswijk | 5:aca2af310419 | 91 | V1 = vx_des; |
nicollevanrijswijk | 5:aca2af310419 | 92 | V2 = vy_des; |
1856413 | 1:2219a519e2bf | 93 | } |
1856413 | 1:2219a519e2bf | 94 | |
nicollevanrijswijk | 5:aca2af310419 | 95 | float IntegrateQ1() |
1856413 | 1:2219a519e2bf | 96 | { |
nicollevanrijswijk | 5:aca2af310419 | 97 | q1 = q1 + (Q1*t); // new value for q1 |
1856413 | 1:2219a519e2bf | 98 | return q1; |
1856413 | 1:2219a519e2bf | 99 | } |
1856413 | 1:2219a519e2bf | 100 | |
nicollevanrijswijk | 5:aca2af310419 | 101 | float IntegrateQ2() |
1856413 | 1:2219a519e2bf | 102 | { |
nicollevanrijswijk | 5:aca2af310419 | 103 | q2 = q2 + (Q2*t); // new value for q2 |
1856413 | 1:2219a519e2bf | 104 | return q2; |
1856413 | 1:2219a519e2bf | 105 | } |
1856413 | 1:2219a519e2bf | 106 | |
nicollevanrijswijk | 5:aca2af310419 | 107 | void ComputeQ_set() |
1856413 | 1:2219a519e2bf | 108 | { |
nicollevanrijswijk | 5:aca2af310419 | 109 | Q1 = (J1*V1) + (J2*V2); |
nicollevanrijswijk | 5:aca2af310419 | 110 | Q2 = (J3*V1) + (J4*V2); |
1856413 | 1:2219a519e2bf | 111 | } |
1856413 | 1:2219a519e2bf | 112 | |
nicollevanrijswijk | 5:aca2af310419 | 113 | float ErrorInverseKinematics1() |
1856413 | 1:2219a519e2bf | 114 | { |
nicollevanrijswijk | 5:aca2af310419 | 115 | float errorIK1 = q1 - currentPosition1; |
1856413 | 1:2219a519e2bf | 116 | return errorIK1; |
1856413 | 0:550f6e86da32 | 117 | } |
1856413 | 4:854aa2e7eeb2 | 118 | |
nicollevanrijswijk | 5:aca2af310419 | 119 | float ErrorInverseKinematics2() |
1856413 | 0:550f6e86da32 | 120 | { |
nicollevanrijswijk | 5:aca2af310419 | 121 | float errorIK2 = q2 - currentPosition2; |
1856413 | 1:2219a519e2bf | 122 | return errorIK2; |
1856413 | 1:2219a519e2bf | 123 | } |
1856413 | 1:2219a519e2bf | 124 | //------------------------------------------------------------------------------ |
1856413 | 1:2219a519e2bf | 125 | // MOTOR PART |
1856413 | 8:3d2228402c71 | 126 | // Failure function |
1856413 | 8:3d2228402c71 | 127 | void TurnMotorsOff() // Turn motors off immediately |
1856413 | 8:3d2228402c71 | 128 | { |
1856413 | 8:3d2228402c71 | 129 | motor1PWM = 0; |
1856413 | 8:3d2228402c71 | 130 | motor2PWM = 0; |
1856413 | 8:3d2228402c71 | 131 | } |
1856413 | 8:3d2228402c71 | 132 | |
1856413 | 1:2219a519e2bf | 133 | //Encoder Posities |
nicollevanrijswijk | 5:aca2af310419 | 134 | void MeasureEncoderPosition1() // Read current position of motor 1, returns value in [rad] |
1856413 | 1:2219a519e2bf | 135 | { |
nicollevanrijswijk | 5:aca2af310419 | 136 | counts1i = Encoder2.getPulses(); |
nicollevanrijswijk | 5:aca2af310419 | 137 | float counts1 = counts1i*1.0f; |
nicollevanrijswijk | 5:aca2af310419 | 138 | currentPosition1 = (counts1/8400)*6.28; //Rotational position in radians |
1856413 | 0:550f6e86da32 | 139 | } |
1856413 | 0:550f6e86da32 | 140 | |
nicollevanrijswijk | 5:aca2af310419 | 141 | void MeasureEncoderPosition2() // Read current postion of motor 2, returns value in [rad] |
1856413 | 1:2219a519e2bf | 142 | { |
nicollevanrijswijk | 5:aca2af310419 | 143 | counts2i = Encoder2.getPulses(); |
nicollevanrijswijk | 5:aca2af310419 | 144 | float counts2 = counts2i*1.0f; |
nicollevanrijswijk | 5:aca2af310419 | 145 | currentPosition2 = (counts2/8400)*6.28; //Rotational position in radians |
1856413 | 1:2219a519e2bf | 146 | } |
1856413 | 1:2219a519e2bf | 147 | |
nicollevanrijswijk | 5:aca2af310419 | 148 | float FeedbackControl1(float Error1) |
1856413 | 0:550f6e86da32 | 149 | { |
nicollevanrijswijk | 5:aca2af310419 | 150 | static float Error_integral1 = 0; |
nicollevanrijswijk | 5:aca2af310419 | 151 | static float Error_prev1 = Error1; |
1856413 | 0:550f6e86da32 | 152 | // Proportional part: |
nicollevanrijswijk | 5:aca2af310419 | 153 | float u_k1 = Kp * Error1; |
1856413 | 0:550f6e86da32 | 154 | // Integral part: |
1856413 | 0:550f6e86da32 | 155 | Error_integral1 = Error_integral1 + Error1 * Ts; |
nicollevanrijswijk | 5:aca2af310419 | 156 | float u_i1 = Ki * Error_integral1; |
1856413 | 1:2219a519e2bf | 157 | // Derivative part: |
nicollevanrijswijk | 5:aca2af310419 | 158 | float Error_derivative1 = (Error1 - Error_prev1)/Ts; |
nicollevanrijswijk | 5:aca2af310419 | 159 | float u_d1 = Kd * Error_derivative1; |
1856413 | 0:550f6e86da32 | 160 | Error_prev1 = Error1; |
1856413 | 0:550f6e86da32 | 161 | // Sum all parts and return it |
1856413 | 0:550f6e86da32 | 162 | return u_k1 + u_i1 + u_d1; //motorValue |
1856413 | 0:550f6e86da32 | 163 | } |
1856413 | 1:2219a519e2bf | 164 | |
nicollevanrijswijk | 5:aca2af310419 | 165 | float FeedbackControl2(float Error2) |
1856413 | 0:550f6e86da32 | 166 | { |
nicollevanrijswijk | 5:aca2af310419 | 167 | static float Error_integral2 = 0; |
nicollevanrijswijk | 5:aca2af310419 | 168 | static float Error_prev2 = Error2; |
1856413 | 0:550f6e86da32 | 169 | // Proportional part: |
nicollevanrijswijk | 5:aca2af310419 | 170 | float u_k2 = Kp * Error2; |
1856413 | 0:550f6e86da32 | 171 | // Integral part: |
1856413 | 0:550f6e86da32 | 172 | Error_integral2 = Error_integral2 + Error2 * Ts; |
nicollevanrijswijk | 5:aca2af310419 | 173 | float u_i2 = Ki * Error_integral2; |
1856413 | 1:2219a519e2bf | 174 | // Derivative part: |
nicollevanrijswijk | 5:aca2af310419 | 175 | float Error_derivative2 = (Error2 - Error_prev2)/Ts; |
nicollevanrijswijk | 5:aca2af310419 | 176 | float u_d2 = Kd * Error_derivative2; |
1856413 | 0:550f6e86da32 | 177 | Error_prev2 = Error2; |
1856413 | 0:550f6e86da32 | 178 | // Sum all parts and return it |
1856413 | 0:550f6e86da32 | 179 | return u_k2 + u_i2 + u_d2; //motorValue |
1856413 | 0:550f6e86da32 | 180 | } |
1856413 | 1:2219a519e2bf | 181 | |
nicollevanrijswijk | 5:aca2af310419 | 182 | void SetMotor1(float motorValue1) |
1856413 | 0:550f6e86da32 | 183 | { |
1856413 | 0:550f6e86da32 | 184 | // Given -1<=motorValue<=1, this sets the PWM and direction |
1856413 | 0:550f6e86da32 | 185 | // bits for motor 1. Positive value makes motor rotating |
1856413 | 0:550f6e86da32 | 186 | // clockwise. motorValues outside range are truncated to |
1856413 | 0:550f6e86da32 | 187 | // within range |
1856413 | 0:550f6e86da32 | 188 | // 0 = clockwise motor direction |
1856413 | 0:550f6e86da32 | 189 | // 1 = counterclockwise motor direction |
1856413 | 0:550f6e86da32 | 190 | if (motorValue1 >=0) { |
1856413 | 0:550f6e86da32 | 191 | motor1Direction=0; |
1856413 | 0:550f6e86da32 | 192 | } else { |
1856413 | 0:550f6e86da32 | 193 | motor1Direction=1; |
1856413 | 0:550f6e86da32 | 194 | } |
1856413 | 0:550f6e86da32 | 195 | if (fabs(motorValue1)>1) { |
1856413 | 0:550f6e86da32 | 196 | motor1PWM = 1; |
1856413 | 0:550f6e86da32 | 197 | } else { |
1856413 | 0:550f6e86da32 | 198 | motor1PWM = fabs(motorValue1); |
1856413 | 0:550f6e86da32 | 199 | } |
1856413 | 0:550f6e86da32 | 200 | } |
1856413 | 1:2219a519e2bf | 201 | |
nicollevanrijswijk | 5:aca2af310419 | 202 | void SetMotor2(float motorValue2) |
1856413 | 0:550f6e86da32 | 203 | { |
1856413 | 0:550f6e86da32 | 204 | // Given -1<=motorValue<=1, this sets the PWM and direction |
1856413 | 0:550f6e86da32 | 205 | // bits for motor 1. Positive value makes motor rotating |
1856413 | 0:550f6e86da32 | 206 | // clockwise. motorValues outside range are truncated to |
1856413 | 0:550f6e86da32 | 207 | // within range |
1856413 | 0:550f6e86da32 | 208 | // 0 = counterclockwise motor direction |
1856413 | 0:550f6e86da32 | 209 | // 1 = clockwise motor direction |
1856413 | 0:550f6e86da32 | 210 | if (motorValue2 >=0) { |
1856413 | 0:550f6e86da32 | 211 | motor2Direction=1; |
1856413 | 0:550f6e86da32 | 212 | } else { |
1856413 | 0:550f6e86da32 | 213 | motor2Direction=0; |
1856413 | 0:550f6e86da32 | 214 | } |
1856413 | 0:550f6e86da32 | 215 | if (fabs(motorValue2)>1) { |
1856413 | 0:550f6e86da32 | 216 | motor2PWM = 1; |
1856413 | 0:550f6e86da32 | 217 | } else { |
1856413 | 0:550f6e86da32 | 218 | motor2PWM = fabs(motorValue2); |
1856413 | 0:550f6e86da32 | 219 | } |
1856413 | 0:550f6e86da32 | 220 | } |
1856413 | 1:2219a519e2bf | 221 | |
1856413 | 8:3d2228402c71 | 222 | void MeasureAndInverseK() |
1856413 | 4:854aa2e7eeb2 | 223 | { |
nicollevanrijswijk | 5:aca2af310419 | 224 | MeasureEncoderPosition1(); // You want to know the current angle of the motors to get the right Jacobian |
nicollevanrijswijk | 5:aca2af310419 | 225 | MeasureEncoderPosition2(); // You want to know the current angle of the motors to get the right Jacobian |
nicollevanrijswijk | 5:aca2af310419 | 226 | ComputeJ(); // Compute matrix J (inverse Jacobian, and Twist is already left out) |
nicollevanrijswijk | 5:aca2af310419 | 227 | ComputeV(); // Compute matrix V (stores the desired velocities obtained from the EMG signal) |
nicollevanrijswijk | 5:aca2af310419 | 228 | ComputeQ_set(); // Compute Q_set (stores Q1 and Q2) |
1856413 | 4:854aa2e7eeb2 | 229 | q1 = IntegrateQ1(); // Compute required angle to go to desired position of end-effector |
1856413 | 4:854aa2e7eeb2 | 230 | q2 = IntegrateQ2(); // Compute required angle to go to desired position of end-effector |
1856413 | 8:3d2228402c71 | 231 | if (q1 < 1.047) { // q1 can only be smaller than 1.047 rad |
1856413 | 8:3d2228402c71 | 232 | q1 = q1; |
1856413 | 8:3d2228402c71 | 233 | } else { // If value of q1 is greater than 1.047 rad, then stay at maximum angle |
1856413 | 8:3d2228402c71 | 234 | q1 = 1.047; |
1856413 | 8:3d2228402c71 | 235 | } |
1856413 | 8:3d2228402c71 | 236 | if (q2 < 0.61) { // q2 cannot be smaller than 0.61 rad |
1856413 | 8:3d2228402c71 | 237 | q2 = 0.61; // Stay at mimimum angle |
1856413 | 8:3d2228402c71 | 238 | } else if (q2 > 1.57) { // q2 cannot be greater than 1.57 rad |
1856413 | 8:3d2228402c71 | 239 | q2 = 1.57; // Stay at maximum angle |
1856413 | 8:3d2228402c71 | 240 | } else { // If q2 is in the right range, let calculated q2 be q2 |
1856413 | 8:3d2228402c71 | 241 | q2 = q2; |
1856413 | 8:3d2228402c71 | 242 | } |
1856413 | 8:3d2228402c71 | 243 | |
nicollevanrijswijk | 5:aca2af310419 | 244 | // Determine error and add PID control |
1856413 | 4:854aa2e7eeb2 | 245 | errorIK1 = ErrorInverseKinematics1(); // Determine difference between current angle motor 1 and desired angle |
1856413 | 4:854aa2e7eeb2 | 246 | errorIK2 = ErrorInverseKinematics2(); // Determine difference between current angle motor 2 and desired angle |
nicollevanrijswijk | 5:aca2af310419 | 247 | // Determine motorValues |
1856413 | 4:854aa2e7eeb2 | 248 | motorValue1 = FeedbackControl1(errorIK1); // Calculate motorValue1 |
1856413 | 4:854aa2e7eeb2 | 249 | motorValue2 = FeedbackControl2(errorIK2); // Calculate motorValue2 |
1856413 | 8:3d2228402c71 | 250 | if (errorIK1<0.5); |
1856413 | 8:3d2228402c71 | 251 | { |
1856413 | 8:3d2228402c71 | 252 | TurnMotorsOff(); |
1856413 | 8:3d2228402c71 | 253 | } |
nicollevanrijswijk | 5:aca2af310419 | 254 | // Make Motor move |
1856413 | 4:854aa2e7eeb2 | 255 | SetMotor1(motorValue1); |
1856413 | 4:854aa2e7eeb2 | 256 | SetMotor2(motorValue2); |
1856413 | 0:550f6e86da32 | 257 | |
nicollevanrijswijk | 5:aca2af310419 | 258 | // Press button to switch velocity direction |
1856413 | 8:3d2228402c71 | 259 | if (!button2.read()) { //keep pressing button2 |
1856413 | 4:854aa2e7eeb2 | 260 | vy_des=vx_des; |
nicollevanrijswijk | 5:aca2af310419 | 261 | vx_des=0.0; |
1856413 | 4:854aa2e7eeb2 | 262 | } else { |
nicollevanrijswijk | 5:aca2af310419 | 263 | vy_des=0.0; |
1856413 | 4:854aa2e7eeb2 | 264 | } |
1856413 | 4:854aa2e7eeb2 | 265 | } |
1856413 | 4:854aa2e7eeb2 | 266 | |
1856413 | 8:3d2228402c71 | 267 | /*void PrintValues() |
1856413 | 4:854aa2e7eeb2 | 268 | { |
1856413 | 4:854aa2e7eeb2 | 269 | pc.printf("vx_des = %f \t vy_des = %f \r\n", vx_des, vy_des); |
1856413 | 4:854aa2e7eeb2 | 270 | pc.printf("Error IK1 = %f \t Error IK2 = %f \r\n", errorIK1, errorIK2); |
1856413 | 4:854aa2e7eeb2 | 271 | pc.printf("MotorValue 1 = %f \t MotorValue 2 = %f \r\n", motorValue1, motorValue2); |
nicollevanrijswijk | 5:aca2af310419 | 272 | pc.printf("q1 Qset = %f \t q2 Qset = %f \r\n", Q1, Q2); |
1856413 | 8:3d2228402c71 | 273 | }*/ |
1856413 | 1:2219a519e2bf | 274 | //------------------------------------------------------------------------------ |
1856413 | 1:2219a519e2bf | 275 | // MAIN |
1856413 | 1:2219a519e2bf | 276 | int main() |
1856413 | 1:2219a519e2bf | 277 | { |
1856413 | 0:550f6e86da32 | 278 | pc.baud(115200); |
1856413 | 8:3d2228402c71 | 279 | potmeterTicker.attach(GetPotMeterVelocity1, 0.02); |
1856413 | 8:3d2228402c71 | 280 | measurecontrolTicker.attach(MeasureAndInverseK, 0.01); |
1856413 | 8:3d2228402c71 | 281 | /*printTicker.attach(printen, 0.5);*/ |
1856413 | 4:854aa2e7eeb2 | 282 | |
1856413 | 1:2219a519e2bf | 283 | while (true) { |
nicollevanrijswijk | 5:aca2af310419 | 284 | Led = !Led; |
nicollevanrijswijk | 5:aca2af310419 | 285 | wait(0.5); |
1856413 | 0:550f6e86da32 | 286 | } |
1856413 | 0:550f6e86da32 | 287 | } |