Control up to two motors using filtered EMG signals and a PID controller
Dependencies: FastPWM HIDScope MODSERIAL QEI Matrix biquadFilter controller errorFetch mbed motorConfig refGen MatrixMath inverseKinematics
Fork of Minor_test_serial by
main.cpp@42:ae78ff03d9d6, 2017-10-27 (annotated)
- Committer:
- tvlogman
- Date:
- Fri Oct 27 10:37:43 2017 +0000
- Revision:
- 42:ae78ff03d9d6
- Parent:
- 41:9678fd827d25
- Child:
- 43:dd0888f86357
Working setup to test the motors using the potmeter
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
tvlogman | 33:6f4858b98fe5 | 1 | #include <vector> |
tvlogman | 37:633dd1901681 | 2 | #include <numeric> |
tvlogman | 42:ae78ff03d9d6 | 3 | #include <algorithm> |
vsluiter | 0:c8f15874531b | 4 | #include "mbed.h" |
tvlogman | 39:d065ad7a978d | 5 | #include "Matrix.h" |
vsluiter | 0:c8f15874531b | 6 | #include "MODSERIAL.h" |
tvlogman | 8:0067469c3389 | 7 | #include "HIDScope.h" |
tvlogman | 9:5f0e796c9489 | 8 | #include "QEI.h" |
tvlogman | 15:b76b8cff4d8f | 9 | #include "FastPWM.h" |
tvlogman | 29:9aa4d63a9bd1 | 10 | #include "refGen.h" |
tvlogman | 30:65f0c9ecf810 | 11 | #include "controller.h" |
tvlogman | 31:cc08254ab7b5 | 12 | #include "motorConfig.h" |
tvlogman | 32:1bb406d2b3c3 | 13 | #include "errorFetch.h" |
tvlogman | 39:d065ad7a978d | 14 | #include "BiQuad.h" |
tvlogman | 15:b76b8cff4d8f | 15 | |
tvlogman | 33:6f4858b98fe5 | 16 | // ADJUSTABLE PARAMETERS |
tvlogman | 37:633dd1901681 | 17 | // controller ticker time interval |
tvlogman | 38:f1d2d42a4bdc | 18 | const float Ts = 0.01; |
tvlogman | 27:a4228ea8fb8f | 19 | |
tvlogman | 34:1a70aa045c8f | 20 | // EMG filter parameters |
tvlogman | 37:633dd1901681 | 21 | // calibration time |
tvlogman | 42:ae78ff03d9d6 | 22 | const int calSamples = 1000; |
tvlogman | 37:633dd1901681 | 23 | |
tvlogman | 41:9678fd827d25 | 24 | // Initialize average and max EMG value for calibration to 0 and 1 respectively |
tvlogman | 37:633dd1901681 | 25 | volatile float avgEMGvalue = 0; |
tvlogman | 41:9678fd827d25 | 26 | volatile double maxEMGvalue = 1; |
tvlogman | 34:1a70aa045c8f | 27 | |
tvlogman | 34:1a70aa045c8f | 28 | // high pass |
tvlogman | 39:d065ad7a978d | 29 | BiQuadChain HPbqc; |
tvlogman | 39:d065ad7a978d | 30 | BiQuad HPbq1(0.9837,-1.9674, 0.9837,1.0000,-1.9769,0.9770); |
tvlogman | 39:d065ad7a978d | 31 | BiQuad HPbq2(1.0000, -2.0000, 1.0000, 1.0000, -1.9903, 0.9904); |
tvlogman | 39:d065ad7a978d | 32 | |
tvlogman | 39:d065ad7a978d | 33 | |
tvlogman | 34:1a70aa045c8f | 34 | // low pass |
tvlogman | 39:d065ad7a978d | 35 | BiQuadChain LPbqc; |
tvlogman | 42:ae78ff03d9d6 | 36 | BiQuad LPbq1(1.0e-7*1.2023, 1.0e-7*2.4046, 1.0e-7*1.2023, 1.0000, -1.9313, 0.9327); |
tvlogman | 42:ae78ff03d9d6 | 37 | BiQuad LPbq2(1.0000, 2.0000, 1.0000, 1.0000, -1.9702, 0.9716); |
tvlogman | 34:1a70aa045c8f | 38 | |
tvlogman | 27:a4228ea8fb8f | 39 | // Controller parameters |
tvlogman | 42:ae78ff03d9d6 | 40 | const float k_p = 1; |
tvlogman | 27:a4228ea8fb8f | 41 | const float k_i = 0; // Still needs a reasonable value |
tvlogman | 27:a4228ea8fb8f | 42 | const float k_d = 0; // Again, still need to pick a reasonable value |
tvlogman | 27:a4228ea8fb8f | 43 | |
tvlogman | 33:6f4858b98fe5 | 44 | // Defining motor gear ratio - for BOTH motors as this is the same in the current configuration |
tvlogman | 33:6f4858b98fe5 | 45 | const float gearRatio = 131; |
tvlogman | 33:6f4858b98fe5 | 46 | |
tvlogman | 37:633dd1901681 | 47 | // LOGISTICS |
tvlogman | 37:633dd1901681 | 48 | // Declaring finite-state-machine states |
tvlogman | 37:633dd1901681 | 49 | enum robotStates {KILLED, ACTIVE, CALIBRATING}; |
tvlogman | 37:633dd1901681 | 50 | volatile robotStates currentState = KILLED; |
tvlogman | 37:633dd1901681 | 51 | volatile bool stateChanged = true; |
tvlogman | 33:6f4858b98fe5 | 52 | |
tvlogman | 33:6f4858b98fe5 | 53 | // Declaring a controller ticker and volatile variables to store encoder counts and revs |
tvlogman | 33:6f4858b98fe5 | 54 | Ticker controllerTicker; |
tvlogman | 33:6f4858b98fe5 | 55 | volatile int m1counts = 0; |
tvlogman | 33:6f4858b98fe5 | 56 | volatile int m2counts = 0; |
tvlogman | 33:6f4858b98fe5 | 57 | volatile float m1revs = 0.00; |
tvlogman | 33:6f4858b98fe5 | 58 | volatile float m2revs = 0.00; |
tvlogman | 33:6f4858b98fe5 | 59 | |
tvlogman | 33:6f4858b98fe5 | 60 | // PWM settings |
tvlogman | 33:6f4858b98fe5 | 61 | float pwmPeriod = 1.0/5000.0; |
tvlogman | 33:6f4858b98fe5 | 62 | int frequency_pwm = 10000; //10kHz PWM |
tvlogman | 33:6f4858b98fe5 | 63 | |
tvlogman | 33:6f4858b98fe5 | 64 | // Initializing encoder |
tvlogman | 32:1bb406d2b3c3 | 65 | QEI Encoder1(D12,D13,NC,64, QEI::X4_ENCODING); |
tvlogman | 32:1bb406d2b3c3 | 66 | QEI Encoder2(D11,D10,NC,64, QEI::X4_ENCODING); |
tvlogman | 10:e23cbcdde7e3 | 67 | MODSERIAL pc(USBTX, USBRX); |
tvlogman | 27:a4228ea8fb8f | 68 | HIDScope scope(5); |
tvlogman | 8:0067469c3389 | 69 | |
tvlogman | 14:664870b5d153 | 70 | // Defining inputs |
tvlogman | 14:664870b5d153 | 71 | InterruptIn sw2(SW2); |
tvlogman | 15:b76b8cff4d8f | 72 | InterruptIn sw3(SW3); |
tvlogman | 16:27430afe663e | 73 | InterruptIn button1(D2); |
tvlogman | 28:8cd898ff43a2 | 74 | InterruptIn button2(D3); |
tvlogman | 38:f1d2d42a4bdc | 75 | //AnalogIn pot2(A2); |
tvlogman | 34:1a70aa045c8f | 76 | //AnalogIn emg0( A0 ); |
tvlogman | 32:1bb406d2b3c3 | 77 | //AnalogIn emg1( A1 ); |
tvlogman | 15:b76b8cff4d8f | 78 | |
tvlogman | 37:633dd1901681 | 79 | // Defining LED outputs to indicate robot state-us |
tvlogman | 37:633dd1901681 | 80 | DigitalOut ledG(LED_GREEN); |
tvlogman | 37:633dd1901681 | 81 | DigitalOut ledR(LED_RED); |
tvlogman | 37:633dd1901681 | 82 | DigitalOut ledB(LED_BLUE); |
tvlogman | 37:633dd1901681 | 83 | |
tvlogman | 27:a4228ea8fb8f | 84 | // Setting up HIDscope |
tvlogman | 16:27430afe663e | 85 | volatile float x; |
tvlogman | 27:a4228ea8fb8f | 86 | volatile float y; |
tvlogman | 27:a4228ea8fb8f | 87 | volatile float z; |
tvlogman | 27:a4228ea8fb8f | 88 | volatile float q; |
tvlogman | 27:a4228ea8fb8f | 89 | volatile float k; |
tvlogman | 39:d065ad7a978d | 90 | volatile float w; |
tvlogman | 27:a4228ea8fb8f | 91 | |
tvlogman | 39:d065ad7a978d | 92 | |
tvlogman | 39:d065ad7a978d | 93 | void sendDataToPc(float data1, float data2, float data3, float data4, float data5, float data6){ |
tvlogman | 27:a4228ea8fb8f | 94 | // Capture data |
tvlogman | 27:a4228ea8fb8f | 95 | x = data1; |
tvlogman | 27:a4228ea8fb8f | 96 | y = data2; |
tvlogman | 27:a4228ea8fb8f | 97 | z = data3; |
tvlogman | 27:a4228ea8fb8f | 98 | q = data4; |
tvlogman | 27:a4228ea8fb8f | 99 | k = data5; |
tvlogman | 39:d065ad7a978d | 100 | w = data6; |
tvlogman | 27:a4228ea8fb8f | 101 | scope.set(0, x); |
tvlogman | 27:a4228ea8fb8f | 102 | scope.set(1, y); |
tvlogman | 27:a4228ea8fb8f | 103 | scope.set(2, z); |
tvlogman | 27:a4228ea8fb8f | 104 | scope.set(3, q); |
tvlogman | 27:a4228ea8fb8f | 105 | scope.set(4, z); |
tvlogman | 39:d065ad7a978d | 106 | scope.set(5, w); |
tvlogman | 27:a4228ea8fb8f | 107 | scope.send(); // send what's in scope memory to PC |
tvlogman | 27:a4228ea8fb8f | 108 | } |
tvlogman | 14:664870b5d153 | 109 | |
tvlogman | 7:1bffab95fc5f | 110 | |
tvlogman | 33:6f4858b98fe5 | 111 | // REFERENCE PARAMETERS |
tvlogman | 42:ae78ff03d9d6 | 112 | int posRevRange = 1; // describes the ends of the position range in complete motor output shaft revolutions in both directions |
tvlogman | 42:ae78ff03d9d6 | 113 | const float maxAngle = 1*3.14*posRevRange; // max angle in radians |
tvlogman | 37:633dd1901681 | 114 | |
tvlogman | 20:4ce3fb543a45 | 115 | |
tvlogman | 27:a4228ea8fb8f | 116 | // Function getReferencePosition returns reference angle based on potmeter 1 |
tvlogman | 42:ae78ff03d9d6 | 117 | refGen ref1(A2, maxAngle); |
tvlogman | 42:ae78ff03d9d6 | 118 | refGen ref2(A3, maxAngle); |
tvlogman | 19:f08b5cd2b7ce | 119 | |
tvlogman | 21:d266d1e503ce | 120 | // readEncoder reads counts and revs and logs results to serial window |
tvlogman | 34:1a70aa045c8f | 121 | errorFetch e1(gearRatio, Ts); |
tvlogman | 38:f1d2d42a4bdc | 122 | errorFetch e2(gearRatio, Ts); |
tvlogman | 21:d266d1e503ce | 123 | |
tvlogman | 31:cc08254ab7b5 | 124 | // Generate a PID controller with the specified values of k_p, k_d and k_i |
tvlogman | 30:65f0c9ecf810 | 125 | controller motorController1(k_p, k_d, k_i); |
tvlogman | 38:f1d2d42a4bdc | 126 | controller motorController2(k_p, k_d, k_i); |
tvlogman | 38:f1d2d42a4bdc | 127 | |
tvlogman | 37:633dd1901681 | 128 | motorConfig motor1(D4,D5); |
tvlogman | 37:633dd1901681 | 129 | motorConfig motor2(D7,D6); |
tvlogman | 37:633dd1901681 | 130 | |
tvlogman | 37:633dd1901681 | 131 | // PROBLEM: if I'm processing the state machine in the endless while loop, how can I adjust robot behavior in the ticker (as it'll keep running)? Do I need to also implement it there? If so, why bother with the while(1) in the main function in the first place? |
tvlogman | 19:f08b5cd2b7ce | 132 | void measureAndControl(){ |
tvlogman | 37:633dd1901681 | 133 | // Read encoders and EMG signal (unnfiltered reference) |
tvlogman | 33:6f4858b98fe5 | 134 | m1counts = Encoder1.getPulses(); |
tvlogman | 33:6f4858b98fe5 | 135 | m2counts = Encoder2.getPulses(); |
tvlogman | 40:7418f46a1ac0 | 136 | |
tvlogman | 40:7418f46a1ac0 | 137 | double m1position = e1.fetchMotorPosition(m1counts); |
tvlogman | 40:7418f46a1ac0 | 138 | double m2position = e2.fetchMotorPosition(m2counts); |
tvlogman | 40:7418f46a1ac0 | 139 | |
tvlogman | 41:9678fd827d25 | 140 | // measuring and normalizing EMG signals to use as basis for reference |
tvlogman | 40:7418f46a1ac0 | 141 | |
tvlogman | 42:ae78ff03d9d6 | 142 | double emg1 = ref1.getReference(); |
tvlogman | 42:ae78ff03d9d6 | 143 | double emg2 = ref2.getReference(); |
tvlogman | 41:9678fd827d25 | 144 | |
tvlogman | 40:7418f46a1ac0 | 145 | |
tvlogman | 40:7418f46a1ac0 | 146 | // Filtering the EMG signals |
tvlogman | 42:ae78ff03d9d6 | 147 | |
tvlogman | 42:ae78ff03d9d6 | 148 | double emg1HP = HPbqc.step(emg1); |
tvlogman | 42:ae78ff03d9d6 | 149 | double emg1HP_abs = fabs(emg1HP); |
tvlogman | 42:ae78ff03d9d6 | 150 | double emg1HPLP_abs = LPbqc.step(emg1HP_abs); |
tvlogman | 42:ae78ff03d9d6 | 151 | // thet1 = fabs(thet1); |
tvlogman | 42:ae78ff03d9d6 | 152 | // thet1 = LPbqc.step(thet1); |
tvlogman | 40:7418f46a1ac0 | 153 | |
tvlogman | 39:d065ad7a978d | 154 | |
tvlogman | 42:ae78ff03d9d6 | 155 | double emg2HP = HPbqc.step(emg2); |
tvlogman | 42:ae78ff03d9d6 | 156 | double emg2HP_abs = fabs(emg2); |
tvlogman | 42:ae78ff03d9d6 | 157 | double emg2HPLP_abs = LPbqc.step(emg2HP_abs); |
tvlogman | 42:ae78ff03d9d6 | 158 | // thet2 = LPbqc.step(thet2); |
tvlogman | 41:9678fd827d25 | 159 | |
tvlogman | 41:9678fd827d25 | 160 | // Something worth trying: set a position setpoint that constantly changes but will never be reached until EMG value is 0 as it is computed from the robot's current position |
tvlogman | 42:ae78ff03d9d6 | 161 | // thet1 = m1position + thet1; |
tvlogman | 42:ae78ff03d9d6 | 162 | // thet2 = m1position + thet2; |
tvlogman | 40:7418f46a1ac0 | 163 | |
tvlogman | 41:9678fd827d25 | 164 | // Other possibility: use thet1 and thet2 directly as reference angles. That'll require the user to hold muscle tension to stay in a certain position though |
tvlogman | 41:9678fd827d25 | 165 | |
tvlogman | 38:f1d2d42a4bdc | 166 | |
tvlogman | 37:633dd1901681 | 167 | // Finite state machine |
tvlogman | 37:633dd1901681 | 168 | switch(currentState){ |
tvlogman | 37:633dd1901681 | 169 | case KILLED: |
tvlogman | 37:633dd1901681 | 170 | { |
tvlogman | 37:633dd1901681 | 171 | // Initialization of KILLED state: cut power to both motors |
tvlogman | 37:633dd1901681 | 172 | if(stateChanged){ |
tvlogman | 37:633dd1901681 | 173 | motor1.kill(); |
tvlogman | 38:f1d2d42a4bdc | 174 | motor2.kill(); |
tvlogman | 37:633dd1901681 | 175 | pc.printf("Killed state \r\n"); |
tvlogman | 37:633dd1901681 | 176 | stateChanged = false; |
tvlogman | 37:633dd1901681 | 177 | } |
tvlogman | 37:633dd1901681 | 178 | |
tvlogman | 37:633dd1901681 | 179 | // Send reference data to pc |
tvlogman | 37:633dd1901681 | 180 | |
tvlogman | 37:633dd1901681 | 181 | // Set LED to red |
tvlogman | 37:633dd1901681 | 182 | ledR = 0; |
tvlogman | 37:633dd1901681 | 183 | ledG = 1; |
tvlogman | 37:633dd1901681 | 184 | ledB = 1; |
tvlogman | 37:633dd1901681 | 185 | // need something here to check if "killswitch" has been pressed (?) |
tvlogman | 37:633dd1901681 | 186 | // NOTE: state transition is handled using buttons triggering functions motorConfig::kill() and motorConfig::turnMotorOn |
tvlogman | 42:ae78ff03d9d6 | 187 | // e1.fetchError(m1position, thet1); |
tvlogman | 42:ae78ff03d9d6 | 188 | // e2.fetchError(m2position, thet1); |
tvlogman | 38:f1d2d42a4bdc | 189 | |
tvlogman | 42:ae78ff03d9d6 | 190 | sendDataToPc(emg1, emg1HP_abs, emg1HPLP_abs, emg2, emg2HP, emg2HPLP_abs); // just send the EMG signal value to HIDscope |
tvlogman | 38:f1d2d42a4bdc | 191 | |
tvlogman | 37:633dd1901681 | 192 | break; |
tvlogman | 37:633dd1901681 | 193 | } |
tvlogman | 37:633dd1901681 | 194 | case ACTIVE: |
tvlogman | 37:633dd1901681 | 195 | { |
tvlogman | 37:633dd1901681 | 196 | if(stateChanged){ |
tvlogman | 37:633dd1901681 | 197 | pc.printf("Active state \r\n"); |
tvlogman | 37:633dd1901681 | 198 | } |
tvlogman | 38:f1d2d42a4bdc | 199 | |
tvlogman | 37:633dd1901681 | 200 | // Compute error |
tvlogman | 42:ae78ff03d9d6 | 201 | e1.fetchError(m1position, emg1); |
tvlogman | 42:ae78ff03d9d6 | 202 | e2.fetchError(m2position, emg2); |
tvlogman | 37:633dd1901681 | 203 | |
tvlogman | 37:633dd1901681 | 204 | // Compute motor value using controller and set motor |
tvlogman | 38:f1d2d42a4bdc | 205 | float motorValue1 = motorController1.control(e1.e_pos, e1.e_int, e1.e_der); |
tvlogman | 38:f1d2d42a4bdc | 206 | float motorValue2 = motorController2.control(e2.e_pos, e2.e_int, e2.e_der); |
tvlogman | 38:f1d2d42a4bdc | 207 | motor1.setMotor(motorValue1); |
tvlogman | 38:f1d2d42a4bdc | 208 | motor2.setMotor(motorValue2); |
tvlogman | 37:633dd1901681 | 209 | |
tvlogman | 37:633dd1901681 | 210 | // Send data to HIDscope |
tvlogman | 42:ae78ff03d9d6 | 211 | sendDataToPc(emg1, emg2, e1.e_pos, e2.e_pos, motorValue1, motorValue2); |
tvlogman | 37:633dd1901681 | 212 | |
tvlogman | 37:633dd1901681 | 213 | // Set LED to blue |
tvlogman | 37:633dd1901681 | 214 | ledR = 1; |
tvlogman | 37:633dd1901681 | 215 | ledG = 1; |
tvlogman | 37:633dd1901681 | 216 | ledB = 0; |
tvlogman | 37:633dd1901681 | 217 | // NOTE: state transition is handled using buttons triggering functions motorConfig::kill() and motorConfig::turnMotorOn |
tvlogman | 37:633dd1901681 | 218 | break; |
tvlogman | 37:633dd1901681 | 219 | } |
tvlogman | 37:633dd1901681 | 220 | case CALIBRATING: |
tvlogman | 37:633dd1901681 | 221 | { |
tvlogman | 37:633dd1901681 | 222 | // NOTE: maybe wrap this whole calibrating thing in a library? |
tvlogman | 37:633dd1901681 | 223 | |
tvlogman | 37:633dd1901681 | 224 | // Do I need to kill motors here? |
tvlogman | 37:633dd1901681 | 225 | |
tvlogman | 37:633dd1901681 | 226 | |
tvlogman | 37:633dd1901681 | 227 | // Initialization of CALIBRATE state |
tvlogman | 37:633dd1901681 | 228 | static int Ncal = 0; |
tvlogman | 37:633dd1901681 | 229 | std::vector<float> EMGsamples; |
tvlogman | 37:633dd1901681 | 230 | |
tvlogman | 37:633dd1901681 | 231 | if(stateChanged){ |
tvlogman | 37:633dd1901681 | 232 | // Kill motors |
tvlogman | 37:633dd1901681 | 233 | pc.printf("Calibrate state \r\n"); |
tvlogman | 37:633dd1901681 | 234 | motor1.kill(); |
tvlogman | 38:f1d2d42a4bdc | 235 | motor2.kill(); |
tvlogman | 37:633dd1901681 | 236 | |
tvlogman | 37:633dd1901681 | 237 | // Clear sample value vector and reset counter variable |
tvlogman | 37:633dd1901681 | 238 | EMGsamples.clear(); |
tvlogman | 37:633dd1901681 | 239 | Ncal = 0; |
tvlogman | 37:633dd1901681 | 240 | stateChanged = false; |
tvlogman | 37:633dd1901681 | 241 | } |
tvlogman | 37:633dd1901681 | 242 | |
tvlogman | 37:633dd1901681 | 243 | // fill the array with sample data if it is not yet filled |
tvlogman | 37:633dd1901681 | 244 | if(Ncal < calSamples){ |
tvlogman | 40:7418f46a1ac0 | 245 | EMGsamples.push_back(emg1); |
tvlogman | 37:633dd1901681 | 246 | Ncal++; |
tvlogman | 37:633dd1901681 | 247 | } |
tvlogman | 37:633dd1901681 | 248 | // if array is filled compute the mean value and switch to active state |
tvlogman | 37:633dd1901681 | 249 | else { |
tvlogman | 37:633dd1901681 | 250 | // Set new avgEMGvalue |
tvlogman | 37:633dd1901681 | 251 | avgEMGvalue = std::accumulate(EMGsamples.begin(), EMGsamples.end(), 0)/calSamples; // still needs to return this value |
tvlogman | 42:ae78ff03d9d6 | 252 | double maxEMGvalue = *std::max_element(EMGsamples.begin(), EMGsamples.end()); |
tvlogman | 41:9678fd827d25 | 253 | |
tvlogman | 37:633dd1901681 | 254 | |
tvlogman | 37:633dd1901681 | 255 | pc.printf("Avg emg value is %.2f changed state to active", avgEMGvalue); |
tvlogman | 37:633dd1901681 | 256 | // State transition logic |
tvlogman | 37:633dd1901681 | 257 | currentState = ACTIVE; |
tvlogman | 37:633dd1901681 | 258 | stateChanged = true; |
tvlogman | 37:633dd1901681 | 259 | Ncal = 0; |
tvlogman | 37:633dd1901681 | 260 | } |
tvlogman | 37:633dd1901681 | 261 | |
tvlogman | 37:633dd1901681 | 262 | // Set LED to green |
tvlogman | 37:633dd1901681 | 263 | ledR = 1; |
tvlogman | 37:633dd1901681 | 264 | ledG = 0; |
tvlogman | 37:633dd1901681 | 265 | ledB = 1; |
tvlogman | 42:ae78ff03d9d6 | 266 | // sendDataToPc(thet1, thet2, 0.0, 0.0, 0.0, 0.0); |
tvlogman | 37:633dd1901681 | 267 | break; |
tvlogman | 37:633dd1901681 | 268 | } |
tvlogman | 37:633dd1901681 | 269 | } |
tvlogman | 37:633dd1901681 | 270 | |
tvlogman | 37:633dd1901681 | 271 | |
tvlogman | 37:633dd1901681 | 272 | |
tvlogman | 15:b76b8cff4d8f | 273 | } |
tvlogman | 15:b76b8cff4d8f | 274 | |
tvlogman | 38:f1d2d42a4bdc | 275 | void r1SwitchDirection(){ |
tvlogman | 33:6f4858b98fe5 | 276 | ref1.r_direction = !ref1.r_direction; |
tvlogman | 27:a4228ea8fb8f | 277 | pc.printf("Switched reference direction! \r\n"); |
tvlogman | 14:664870b5d153 | 278 | } |
tvlogman | 38:f1d2d42a4bdc | 279 | |
tvlogman | 38:f1d2d42a4bdc | 280 | void r2SwitchDirection(){ |
tvlogman | 38:f1d2d42a4bdc | 281 | ref2.r_direction = !ref2.r_direction; |
tvlogman | 38:f1d2d42a4bdc | 282 | pc.printf("Switched reference direction! \r\n"); |
tvlogman | 38:f1d2d42a4bdc | 283 | } |
vsluiter | 0:c8f15874531b | 284 | |
tvlogman | 37:633dd1901681 | 285 | void killSwitch(){ |
tvlogman | 37:633dd1901681 | 286 | currentState = KILLED; |
tvlogman | 37:633dd1901681 | 287 | stateChanged = true; |
tvlogman | 37:633dd1901681 | 288 | } |
tvlogman | 37:633dd1901681 | 289 | |
tvlogman | 37:633dd1901681 | 290 | void activateRobot(){ |
tvlogman | 37:633dd1901681 | 291 | currentState = ACTIVE; |
tvlogman | 37:633dd1901681 | 292 | stateChanged = true; |
tvlogman | 37:633dd1901681 | 293 | } |
tvlogman | 37:633dd1901681 | 294 | |
tvlogman | 37:633dd1901681 | 295 | void calibrateRobot(){ |
tvlogman | 37:633dd1901681 | 296 | currentState = CALIBRATING; |
tvlogman | 37:633dd1901681 | 297 | stateChanged = true; |
tvlogman | 37:633dd1901681 | 298 | } |
tvlogman | 21:d266d1e503ce | 299 | |
vsluiter | 0:c8f15874531b | 300 | int main() |
tvlogman | 10:e23cbcdde7e3 | 301 | { |
tvlogman | 37:633dd1901681 | 302 | pc.baud(115200); |
tvlogman | 19:f08b5cd2b7ce | 303 | pc.printf("Main function"); |
tvlogman | 39:d065ad7a978d | 304 | HPbqc.add(&HPbq1).add(&HPbq2); |
tvlogman | 39:d065ad7a978d | 305 | LPbqc.add(&LPbq1).add(&LPbq2); |
tvlogman | 39:d065ad7a978d | 306 | |
tvlogman | 37:633dd1901681 | 307 | // Attaching state change functions to buttons; |
tvlogman | 37:633dd1901681 | 308 | sw2.fall(&killSwitch); |
tvlogman | 37:633dd1901681 | 309 | sw3.fall(&activateRobot); |
tvlogman | 38:f1d2d42a4bdc | 310 | button1.rise(&r1SwitchDirection); |
tvlogman | 42:ae78ff03d9d6 | 311 | button2.rise(&r2SwitchDirection); |
tvlogman | 37:633dd1901681 | 312 | |
tvlogman | 22:2e473e9798c0 | 313 | controllerTicker.attach(measureAndControl, Ts); |
tvlogman | 19:f08b5cd2b7ce | 314 | pc.printf("Encoder ticker attached and baudrate set"); |
vsluiter | 0:c8f15874531b | 315 | } |
tvlogman | 7:1bffab95fc5f | 316 |