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