Project BioRobotics Group 19
Dependencies: FastPWM HIDScope MODSERIAL QEI biquadFilter mbed
main.cpp@3:5c3edcd29448, 2017-10-09 (annotated)
- Committer:
- jordiluong
- Date:
- Mon Oct 09 13:59:45 2017 +0000
- Revision:
- 3:5c3edcd29448
- Parent:
- 2:d3687b2c4e37
- Child:
- 4:ea7689bf97e1
Added functions for motors (TEST VERSION)
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jordiluong | 0:80ac024b84cb | 1 | #include "BiQuad.h" |
jordiluong | 0:80ac024b84cb | 2 | #include "FastPWM.h" |
jordiluong | 0:80ac024b84cb | 3 | #include "HIDScope.h" |
jordiluong | 0:80ac024b84cb | 4 | #include "MODSERIAL.h" |
jordiluong | 0:80ac024b84cb | 5 | #include "mbed.h" |
jordiluong | 0:80ac024b84cb | 6 | #include "QEI.h" |
jordiluong | 0:80ac024b84cb | 7 | |
jordiluong | 0:80ac024b84cb | 8 | // SERIAL COMMUNICATION WITH PC |
jordiluong | 0:80ac024b84cb | 9 | MODSERIAL pc(USBTX, USBRX); |
jordiluong | 0:80ac024b84cb | 10 | |
jordiluong | 0:80ac024b84cb | 11 | // STATES |
jordiluong | 3:5c3edcd29448 | 12 | enum states{MOTORS_OFF, MOVING, HITTING}; |
jordiluong | 0:80ac024b84cb | 13 | |
jordiluong | 0:80ac024b84cb | 14 | states currentState = MOTORS_OFF; // Start with motors off |
jordiluong | 0:80ac024b84cb | 15 | bool stateChanged = true; // Make sure the initialization of first state is executed |
jordiluong | 0:80ac024b84cb | 16 | |
jordiluong | 3:5c3edcd29448 | 17 | // PINS |
jordiluong | 3:5c3edcd29448 | 18 | DigitalOut motor1DirectionPin(D4); // Value 0: CW or CCW?; 1: CW or CCW? |
jordiluong | 3:5c3edcd29448 | 19 | PwmOut motor1MagnitudePin(D5); |
jordiluong | 3:5c3edcd29448 | 20 | DigitalOut motor2DirectionPin(D7); // Value 0: CW or CCW?; 1: CW or CCW? |
jordiluong | 3:5c3edcd29448 | 21 | PwmOut motor2MagnitudePin(D6); |
jordiluong | 3:5c3edcd29448 | 22 | InterruptIn button1(D2); // BUTTON 1 AANSLUITEN OP D2!!! |
jordiluong | 3:5c3edcd29448 | 23 | InterruptIn button2(D3); // BUTTON 2 AANSLUITEN OP D3!!! |
jordiluong | 3:5c3edcd29448 | 24 | |
jordiluong | 0:80ac024b84cb | 25 | // DEFINITIONS |
jordiluong | 3:5c3edcd29448 | 26 | const float motorVelocity = 4; // rad/s |
jordiluong | 3:5c3edcd29448 | 27 | const float motorGain = 4; // (rad/s) / PWM |
jordiluong | 0:80ac024b84cb | 28 | |
jordiluong | 0:80ac024b84cb | 29 | // FUNCTIONS |
jordiluong | 0:80ac024b84cb | 30 | // Turn motors off |
jordiluong | 3:5c3edcd29448 | 31 | /*void TurnMotorsOff() |
jordiluong | 0:80ac024b84cb | 32 | { |
jordiluong | 2:d3687b2c4e37 | 33 | // Turn motors off |
jordiluong | 0:80ac024b84cb | 34 | } |
jordiluong | 0:80ac024b84cb | 35 | |
jordiluong | 0:80ac024b84cb | 36 | // Move to home |
jordiluong | 0:80ac024b84cb | 37 | void MoveToHome() |
jordiluong | 0:80ac024b84cb | 38 | { |
jordiluong | 2:d3687b2c4e37 | 39 | // Move to home |
jordiluong | 0:80ac024b84cb | 40 | } |
jordiluong | 0:80ac024b84cb | 41 | |
jordiluong | 0:80ac024b84cb | 42 | // Filter signals |
jordiluong | 2:d3687b2c4e37 | 43 | float FilterSignal(// Signal) |
jordiluong | 0:80ac024b84cb | 44 | { |
jordiluong | 2:d3687b2c4e37 | 45 | // Filter signal |
jordiluong | 2:d3687b2c4e37 | 46 | return // Voltage |
jordiluong | 0:80ac024b84cb | 47 | } |
jordiluong | 0:80ac024b84cb | 48 | |
jordiluong | 0:80ac024b84cb | 49 | // Motor 1 |
jordiluong | 2:d3687b2c4e37 | 50 | void RotateMotor1(// Voltage) |
jordiluong | 0:80ac024b84cb | 51 | { |
jordiluong | 2:d3687b2c4e37 | 52 | // Rotate motor 1 |
jordiluong | 0:80ac024b84cb | 53 | } |
jordiluong | 0:80ac024b84cb | 54 | |
jordiluong | 0:80ac024b84cb | 55 | // Motor 2 |
jordiluong | 2:d3687b2c4e37 | 56 | void RotateMotor2(// Voltage) |
jordiluong | 0:80ac024b84cb | 57 | { |
jordiluong | 2:d3687b2c4e37 | 58 | // Rotate motor 2 |
jordiluong | 0:80ac024b84cb | 59 | } |
jordiluong | 0:80ac024b84cb | 60 | |
jordiluong | 0:80ac024b84cb | 61 | // Hit the ball |
jordiluong | 0:80ac024b84cb | 62 | void HitBall() |
jordiluong | 0:80ac024b84cb | 63 | { |
jordiluong | 2:d3687b2c4e37 | 64 | // Rotate motor 3 |
jordiluong | 0:80ac024b84cb | 65 | } |
jordiluong | 3:5c3edcd29448 | 66 | */ |
jordiluong | 3:5c3edcd29448 | 67 | |
jordiluong | 3:5c3edcd29448 | 68 | void TurnMotor1CW() |
jordiluong | 3:5c3edcd29448 | 69 | { |
jordiluong | 3:5c3edcd29448 | 70 | motor1DirectionPin = 0; |
jordiluong | 3:5c3edcd29448 | 71 | motor1MagnitudePin = fabs(motorVelocity / motorGain); |
jordiluong | 3:5c3edcd29448 | 72 | } |
jordiluong | 3:5c3edcd29448 | 73 | |
jordiluong | 3:5c3edcd29448 | 74 | void TurnMotor1CCW() |
jordiluong | 3:5c3edcd29448 | 75 | { |
jordiluong | 3:5c3edcd29448 | 76 | motor1DirectionPin = 1; |
jordiluong | 3:5c3edcd29448 | 77 | motor1MagnitudePin = fabs(motorVelocity / motorGain); |
jordiluong | 3:5c3edcd29448 | 78 | } |
jordiluong | 3:5c3edcd29448 | 79 | |
jordiluong | 3:5c3edcd29448 | 80 | void TurnMotorsOff() |
jordiluong | 3:5c3edcd29448 | 81 | { |
jordiluong | 3:5c3edcd29448 | 82 | motor1MagnitudePin = 0; |
jordiluong | 3:5c3edcd29448 | 83 | } |
jordiluong | 0:80ac024b84cb | 84 | |
jordiluong | 0:80ac024b84cb | 85 | // States function |
jordiluong | 0:80ac024b84cb | 86 | void ProcessStateMachine() |
jordiluong | 0:80ac024b84cb | 87 | { |
jordiluong | 0:80ac024b84cb | 88 | switch(currentState) |
jordiluong | 0:80ac024b84cb | 89 | { |
jordiluong | 0:80ac024b84cb | 90 | case MOTORS_OFF: |
jordiluong | 0:80ac024b84cb | 91 | { |
jordiluong | 0:80ac024b84cb | 92 | // State initialization |
jordiluong | 0:80ac024b84cb | 93 | if(stateChanged) |
jordiluong | 0:80ac024b84cb | 94 | { |
jordiluong | 0:80ac024b84cb | 95 | pc.printf("Entering MOTORS_OFF \r\n"); |
jordiluong | 0:80ac024b84cb | 96 | TurnMotorsOff(); // Turn motors off |
jordiluong | 0:80ac024b84cb | 97 | stateChanged = false; |
jordiluong | 0:80ac024b84cb | 98 | } |
jordiluong | 0:80ac024b84cb | 99 | |
jordiluong | 0:80ac024b84cb | 100 | // Home command |
jordiluong | 3:5c3edcd29448 | 101 | if(button1) |
jordiluong | 0:80ac024b84cb | 102 | { |
jordiluong | 0:80ac024b84cb | 103 | currentState = MOVING; |
jordiluong | 0:80ac024b84cb | 104 | stateChanged = true; |
jordiluong | 0:80ac024b84cb | 105 | break; |
jordiluong | 0:80ac024b84cb | 106 | } |
jordiluong | 0:80ac024b84cb | 107 | } |
jordiluong | 0:80ac024b84cb | 108 | |
jordiluong | 0:80ac024b84cb | 109 | case MOVING: |
jordiluong | 0:80ac024b84cb | 110 | { |
jordiluong | 0:80ac024b84cb | 111 | // State initialization |
jordiluong | 0:80ac024b84cb | 112 | if(stateChanged) |
jordiluong | 0:80ac024b84cb | 113 | { |
jordiluong | 0:80ac024b84cb | 114 | pc.printf("Entering MOVING \r\n"); |
jordiluong | 0:80ac024b84cb | 115 | stateChanged = false; |
jordiluong | 0:80ac024b84cb | 116 | } |
jordiluong | 3:5c3edcd29448 | 117 | /* |
jordiluong | 0:80ac024b84cb | 118 | // EMG signals to rotate motor 1 |
jordiluong | 2:d3687b2c4e37 | 119 | if(// EMG signal) |
jordiluong | 0:80ac024b84cb | 120 | { |
jordiluong | 2:d3687b2c4e37 | 121 | FilterSignal(// Signal); // Filter the signal |
jordiluong | 2:d3687b2c4e37 | 122 | RotateMotor1(// Voltage); // Rotate motor 1 |
jordiluong | 0:80ac024b84cb | 123 | } |
jordiluong | 0:80ac024b84cb | 124 | |
jordiluong | 0:80ac024b84cb | 125 | // EMG signals to rotate motor 2 |
jordiluong | 2:d3687b2c4e37 | 126 | if(// EMG signal) |
jordiluong | 0:80ac024b84cb | 127 | { |
jordiluong | 2:d3687b2c4e37 | 128 | FilterSignal(// Signal); // Filter the signal |
jordiluong | 2:d3687b2c4e37 | 129 | RotateMotor2(// Voltage); // Rotate motor 2 |
jordiluong | 0:80ac024b84cb | 130 | } |
jordiluong | 0:80ac024b84cb | 131 | |
jordiluong | 0:80ac024b84cb | 132 | // Hit command |
jordiluong | 2:d3687b2c4e37 | 133 | if(// HIT COMMAND) |
jordiluong | 0:80ac024b84cb | 134 | { |
jordiluong | 0:80ac024b84cb | 135 | currentState = HITTING; |
jordiluong | 0:80ac024b84cb | 136 | stateChanged = true; |
jordiluong | 0:80ac024b84cb | 137 | break; |
jordiluong | 0:80ac024b84cb | 138 | } |
jordiluong | 3:5c3edcd29448 | 139 | */ |
jordiluong | 3:5c3edcd29448 | 140 | // Motor testen |
jordiluong | 3:5c3edcd29448 | 141 | while(button1) |
jordiluong | 3:5c3edcd29448 | 142 | { |
jordiluong | 3:5c3edcd29448 | 143 | TurnMotor1CW(); |
jordiluong | 3:5c3edcd29448 | 144 | pc.printf("Turning motor 1 CW \r\n"); |
jordiluong | 3:5c3edcd29448 | 145 | } |
jordiluong | 3:5c3edcd29448 | 146 | |
jordiluong | 3:5c3edcd29448 | 147 | while(button2) |
jordiluong | 3:5c3edcd29448 | 148 | { |
jordiluong | 3:5c3edcd29448 | 149 | TurnMotor1CCW(); |
jordiluong | 3:5c3edcd29448 | 150 | pc.printf("Turning motor 1 CounterCW \r\n"); |
jordiluong | 3:5c3edcd29448 | 151 | } |
jordiluong | 3:5c3edcd29448 | 152 | |
jordiluong | 3:5c3edcd29448 | 153 | TurnMotorsOff(); |
jordiluong | 0:80ac024b84cb | 154 | } |
jordiluong | 0:80ac024b84cb | 155 | |
jordiluong | 0:80ac024b84cb | 156 | case HITTING: |
jordiluong | 0:80ac024b84cb | 157 | { |
jordiluong | 0:80ac024b84cb | 158 | // State initialization |
jordiluong | 0:80ac024b84cb | 159 | if(stateChanged) |
jordiluong | 0:80ac024b84cb | 160 | { |
jordiluong | 0:80ac024b84cb | 161 | pc.printf("Entering HITTING \r\n"); |
jordiluong | 3:5c3edcd29448 | 162 | //HitBall(); // Hit the ball |
jordiluong | 0:80ac024b84cb | 163 | stateChanged = false; |
jordiluong | 0:80ac024b84cb | 164 | currentState = MOVING; |
jordiluong | 0:80ac024b84cb | 165 | stateChanged = true; |
jordiluong | 0:80ac024b84cb | 166 | break; |
jordiluong | 0:80ac024b84cb | 167 | } |
jordiluong | 0:80ac024b84cb | 168 | } |
jordiluong | 0:80ac024b84cb | 169 | |
jordiluong | 0:80ac024b84cb | 170 | default: |
jordiluong | 0:80ac024b84cb | 171 | { |
jordiluong | 0:80ac024b84cb | 172 | TurnMotorsOff(); // Turn motors off for safety |
jordiluong | 0:80ac024b84cb | 173 | } |
jordiluong | 0:80ac024b84cb | 174 | } |
jordiluong | 0:80ac024b84cb | 175 | } |
jordiluong | 0:80ac024b84cb | 176 | |
jordiluong | 0:80ac024b84cb | 177 | // Main function |
jordiluong | 0:80ac024b84cb | 178 | int main() |
jordiluong | 0:80ac024b84cb | 179 | { |
jordiluong | 0:80ac024b84cb | 180 | // Serial communication |
jordiluong | 0:80ac024b84cb | 181 | pc.baud(115200); |
jordiluong | 0:80ac024b84cb | 182 | |
jordiluong | 0:80ac024b84cb | 183 | while(true) |
jordiluong | 0:80ac024b84cb | 184 | { |
jordiluong | 0:80ac024b84cb | 185 | ProcessStateMachine(); // Execute states function |
jordiluong | 0:80ac024b84cb | 186 | } |
jordiluong | 0:80ac024b84cb | 187 | } |