Project BioRobotics Group 19
Dependencies: FastPWM HIDScope MODSERIAL QEI biquadFilter mbed
main.cpp@4:ea7689bf97e1, 2017-10-11 (annotated)
- Committer:
- jordiluong
- Date:
- Wed Oct 11 10:33:11 2017 +0000
- Revision:
- 4:ea7689bf97e1
- Parent:
- 3:5c3edcd29448
- Child:
- 5:0d3e8694726e
- Child:
- 6:246b05228f52
Motors working with buttons
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 | 4:ea7689bf97e1 | 17 | // ENCODER |
jordiluong | 4:ea7689bf97e1 | 18 | QEI Encoder(D12,D13,NC,32); // CONNECT ENC2A TO D13, ENC2B TO D12 |
jordiluong | 4:ea7689bf97e1 | 19 | float vorig_omwentelingen = 0; |
jordiluong | 4:ea7689bf97e1 | 20 | float degrees; |
jordiluong | 4:ea7689bf97e1 | 21 | float vorig_degrees = 0; |
jordiluong | 4:ea7689bf97e1 | 22 | float omwentelingen; |
jordiluong | 4:ea7689bf97e1 | 23 | float counts; |
jordiluong | 4:ea7689bf97e1 | 24 | float snelheid_tpm; |
jordiluong | 4:ea7689bf97e1 | 25 | float snelheid_degps; |
jordiluong | 4:ea7689bf97e1 | 26 | float snelheid_tps; |
jordiluong | 4:ea7689bf97e1 | 27 | |
jordiluong | 3:5c3edcd29448 | 28 | // PINS |
jordiluong | 4:ea7689bf97e1 | 29 | DigitalOut motor1DirectionPin(D4); // Value 0: CCW; 1: CW |
jordiluong | 3:5c3edcd29448 | 30 | PwmOut motor1MagnitudePin(D5); |
jordiluong | 3:5c3edcd29448 | 31 | DigitalOut motor2DirectionPin(D7); // Value 0: CW or CCW?; 1: CW or CCW? |
jordiluong | 3:5c3edcd29448 | 32 | PwmOut motor2MagnitudePin(D6); |
jordiluong | 4:ea7689bf97e1 | 33 | InterruptIn button1(D2); // CONNECT BUT1 TO D2 |
jordiluong | 4:ea7689bf97e1 | 34 | InterruptIn button2(D3); // CONNECT BUT2 TO D3 |
jordiluong | 3:5c3edcd29448 | 35 | |
jordiluong | 0:80ac024b84cb | 36 | // DEFINITIONS |
jordiluong | 4:ea7689bf97e1 | 37 | const float motorVelocity = 1; // unit: rad/s |
jordiluong | 4:ea7689bf97e1 | 38 | const float motorGain = 8.4; // unit: (rad/s) / PWM |
jordiluong | 4:ea7689bf97e1 | 39 | const int motorRatio = 131; // Ratio of the gearbox in the motors |
jordiluong | 4:ea7689bf97e1 | 40 | int motor1State = 0; // 0: Off, 1: CW, 2: CCW |
jordiluong | 4:ea7689bf97e1 | 41 | |
jordiluong | 4:ea7689bf97e1 | 42 | //TICKERS |
jordiluong | 4:ea7689bf97e1 | 43 | Ticker encoder; |
jordiluong | 4:ea7689bf97e1 | 44 | Ticker checkMotorState; |
jordiluong | 0:80ac024b84cb | 45 | |
jordiluong | 0:80ac024b84cb | 46 | // FUNCTIONS |
jordiluong | 4:ea7689bf97e1 | 47 | void RotateMotor1CW() |
jordiluong | 0:80ac024b84cb | 48 | { |
jordiluong | 4:ea7689bf97e1 | 49 | motor1DirectionPin = 1; |
jordiluong | 4:ea7689bf97e1 | 50 | motor1MagnitudePin = fabs(motorVelocity / motorGain); |
jordiluong | 4:ea7689bf97e1 | 51 | //pc.printf("Rotating motor 1 CW \r\n"); |
jordiluong | 0:80ac024b84cb | 52 | } |
jordiluong | 0:80ac024b84cb | 53 | |
jordiluong | 4:ea7689bf97e1 | 54 | void RotateMotor1CCW() |
jordiluong | 0:80ac024b84cb | 55 | { |
jordiluong | 4:ea7689bf97e1 | 56 | motor1DirectionPin = 0; |
jordiluong | 4:ea7689bf97e1 | 57 | motor1MagnitudePin = fabs(motorVelocity / motorGain); |
jordiluong | 4:ea7689bf97e1 | 58 | //pc.printf("Rotating motor 1 CounterCW \r\n"); |
jordiluong | 0:80ac024b84cb | 59 | } |
jordiluong | 0:80ac024b84cb | 60 | |
jordiluong | 4:ea7689bf97e1 | 61 | void TurnMotor1Off() |
jordiluong | 0:80ac024b84cb | 62 | { |
jordiluong | 4:ea7689bf97e1 | 63 | motor1MagnitudePin = 0; |
jordiluong | 4:ea7689bf97e1 | 64 | //pc.printf("Motors off \r\n"); |
jordiluong | 0:80ac024b84cb | 65 | } |
jordiluong | 0:80ac024b84cb | 66 | |
jordiluong | 4:ea7689bf97e1 | 67 | void CheckMotor1() // Checks the state of motor1 and rotates motor1 depending on the state |
jordiluong | 0:80ac024b84cb | 68 | { |
jordiluong | 4:ea7689bf97e1 | 69 | switch(motor1State) |
jordiluong | 4:ea7689bf97e1 | 70 | { |
jordiluong | 4:ea7689bf97e1 | 71 | case 0: // Turn motors off |
jordiluong | 4:ea7689bf97e1 | 72 | {TurnMotor1Off(); break;} |
jordiluong | 4:ea7689bf97e1 | 73 | |
jordiluong | 4:ea7689bf97e1 | 74 | case 1: // Rotate motor 1 CW |
jordiluong | 4:ea7689bf97e1 | 75 | {RotateMotor1CW(); break;} |
jordiluong | 4:ea7689bf97e1 | 76 | |
jordiluong | 4:ea7689bf97e1 | 77 | case 2: // Rotate motor 2 CCW |
jordiluong | 4:ea7689bf97e1 | 78 | {RotateMotor1CCW(); break;} |
jordiluong | 4:ea7689bf97e1 | 79 | |
jordiluong | 4:ea7689bf97e1 | 80 | default: |
jordiluong | 4:ea7689bf97e1 | 81 | break; |
jordiluong | 4:ea7689bf97e1 | 82 | } |
jordiluong | 3:5c3edcd29448 | 83 | } |
jordiluong | 3:5c3edcd29448 | 84 | |
jordiluong | 4:ea7689bf97e1 | 85 | void EncoderCalc() |
jordiluong | 3:5c3edcd29448 | 86 | { |
jordiluong | 4:ea7689bf97e1 | 87 | counts = Encoder.getPulses()/motorRatio; |
jordiluong | 4:ea7689bf97e1 | 88 | degrees = counts/32*360; |
jordiluong | 4:ea7689bf97e1 | 89 | omwentelingen = counts/32; |
jordiluong | 4:ea7689bf97e1 | 90 | snelheid_tpm = (omwentelingen-vorig_omwentelingen)/0.5*60; |
jordiluong | 4:ea7689bf97e1 | 91 | snelheid_tps = (omwentelingen-vorig_omwentelingen)/0.5; |
jordiluong | 4:ea7689bf97e1 | 92 | snelheid_degps = (degrees-vorig_degrees)/0.5; |
jordiluong | 4:ea7689bf97e1 | 93 | vorig_omwentelingen = omwentelingen; |
jordiluong | 4:ea7689bf97e1 | 94 | vorig_degrees = degrees; |
jordiluong | 4:ea7689bf97e1 | 95 | //pc.printf("%.1f pulsen, %.2f graden, %.2f omwentelingen, %.2f tpm, %.2f tps, %.2f deg/s\r\n",counts, degrees, omwentelingen, snelheid_tpm, snelheid_tps, snelheid_degps); |
jordiluong | 3:5c3edcd29448 | 96 | } |
jordiluong | 0:80ac024b84cb | 97 | |
jordiluong | 0:80ac024b84cb | 98 | // States function |
jordiluong | 0:80ac024b84cb | 99 | void ProcessStateMachine() |
jordiluong | 0:80ac024b84cb | 100 | { |
jordiluong | 0:80ac024b84cb | 101 | switch(currentState) |
jordiluong | 0:80ac024b84cb | 102 | { |
jordiluong | 0:80ac024b84cb | 103 | case MOTORS_OFF: |
jordiluong | 0:80ac024b84cb | 104 | { |
jordiluong | 0:80ac024b84cb | 105 | // State initialization |
jordiluong | 0:80ac024b84cb | 106 | if(stateChanged) |
jordiluong | 0:80ac024b84cb | 107 | { |
jordiluong | 0:80ac024b84cb | 108 | pc.printf("Entering MOTORS_OFF \r\n"); |
jordiluong | 4:ea7689bf97e1 | 109 | TurnMotor1Off(); // Turn motors off |
jordiluong | 0:80ac024b84cb | 110 | stateChanged = false; |
jordiluong | 0:80ac024b84cb | 111 | } |
jordiluong | 0:80ac024b84cb | 112 | |
jordiluong | 0:80ac024b84cb | 113 | // Home command |
jordiluong | 4:ea7689bf97e1 | 114 | if(!button1) |
jordiluong | 0:80ac024b84cb | 115 | { |
jordiluong | 0:80ac024b84cb | 116 | currentState = MOVING; |
jordiluong | 0:80ac024b84cb | 117 | stateChanged = true; |
jordiluong | 0:80ac024b84cb | 118 | break; |
jordiluong | 0:80ac024b84cb | 119 | } |
jordiluong | 4:ea7689bf97e1 | 120 | break; |
jordiluong | 0:80ac024b84cb | 121 | } |
jordiluong | 0:80ac024b84cb | 122 | |
jordiluong | 0:80ac024b84cb | 123 | case MOVING: |
jordiluong | 0:80ac024b84cb | 124 | { |
jordiluong | 0:80ac024b84cb | 125 | // State initialization |
jordiluong | 0:80ac024b84cb | 126 | if(stateChanged) |
jordiluong | 0:80ac024b84cb | 127 | { |
jordiluong | 0:80ac024b84cb | 128 | pc.printf("Entering MOVING \r\n"); |
jordiluong | 0:80ac024b84cb | 129 | stateChanged = false; |
jordiluong | 0:80ac024b84cb | 130 | } |
jordiluong | 4:ea7689bf97e1 | 131 | |
jordiluong | 4:ea7689bf97e1 | 132 | // Move commands |
jordiluong | 0:80ac024b84cb | 133 | |
jordiluong | 4:ea7689bf97e1 | 134 | if(!button1){motor1State = 1;} |
jordiluong | 4:ea7689bf97e1 | 135 | else if(!button2){motor1State = 2;} |
jordiluong | 4:ea7689bf97e1 | 136 | else{motor1State = 0;} |
jordiluong | 0:80ac024b84cb | 137 | |
jordiluong | 4:ea7689bf97e1 | 138 | /* |
jordiluong | 0:80ac024b84cb | 139 | // Hit command |
jordiluong | 2:d3687b2c4e37 | 140 | if(// HIT COMMAND) |
jordiluong | 0:80ac024b84cb | 141 | { |
jordiluong | 0:80ac024b84cb | 142 | currentState = HITTING; |
jordiluong | 0:80ac024b84cb | 143 | stateChanged = true; |
jordiluong | 0:80ac024b84cb | 144 | break; |
jordiluong | 0:80ac024b84cb | 145 | } |
jordiluong | 3:5c3edcd29448 | 146 | */ |
jordiluong | 4:ea7689bf97e1 | 147 | break; |
jordiluong | 0:80ac024b84cb | 148 | } |
jordiluong | 0:80ac024b84cb | 149 | |
jordiluong | 0:80ac024b84cb | 150 | case HITTING: |
jordiluong | 0:80ac024b84cb | 151 | { |
jordiluong | 0:80ac024b84cb | 152 | // State initialization |
jordiluong | 0:80ac024b84cb | 153 | if(stateChanged) |
jordiluong | 0:80ac024b84cb | 154 | { |
jordiluong | 0:80ac024b84cb | 155 | pc.printf("Entering HITTING \r\n"); |
jordiluong | 4:ea7689bf97e1 | 156 | //HitBall(); // Hit the ball |
jordiluong | 0:80ac024b84cb | 157 | stateChanged = false; |
jordiluong | 0:80ac024b84cb | 158 | currentState = MOVING; |
jordiluong | 0:80ac024b84cb | 159 | stateChanged = true; |
jordiluong | 0:80ac024b84cb | 160 | break; |
jordiluong | 0:80ac024b84cb | 161 | } |
jordiluong | 4:ea7689bf97e1 | 162 | break; |
jordiluong | 0:80ac024b84cb | 163 | } |
jordiluong | 0:80ac024b84cb | 164 | |
jordiluong | 0:80ac024b84cb | 165 | default: |
jordiluong | 0:80ac024b84cb | 166 | { |
jordiluong | 4:ea7689bf97e1 | 167 | TurnMotor1Off(); // Turn motors off for safety |
jordiluong | 4:ea7689bf97e1 | 168 | break; |
jordiluong | 0:80ac024b84cb | 169 | } |
jordiluong | 0:80ac024b84cb | 170 | } |
jordiluong | 0:80ac024b84cb | 171 | } |
jordiluong | 0:80ac024b84cb | 172 | |
jordiluong | 0:80ac024b84cb | 173 | // Main function |
jordiluong | 0:80ac024b84cb | 174 | int main() |
jordiluong | 0:80ac024b84cb | 175 | { |
jordiluong | 0:80ac024b84cb | 176 | // Serial communication |
jordiluong | 0:80ac024b84cb | 177 | pc.baud(115200); |
jordiluong | 0:80ac024b84cb | 178 | |
jordiluong | 4:ea7689bf97e1 | 179 | pc.printf("START \r\n"); |
jordiluong | 4:ea7689bf97e1 | 180 | |
jordiluong | 4:ea7689bf97e1 | 181 | encoder.attach(EncoderCalc, 0.5); |
jordiluong | 4:ea7689bf97e1 | 182 | checkMotorState.attach(CheckMotor1, 0.01); |
jordiluong | 4:ea7689bf97e1 | 183 | |
jordiluong | 0:80ac024b84cb | 184 | while(true) |
jordiluong | 0:80ac024b84cb | 185 | { |
jordiluong | 0:80ac024b84cb | 186 | ProcessStateMachine(); // Execute states function |
jordiluong | 0:80ac024b84cb | 187 | } |
jordiluong | 0:80ac024b84cb | 188 | } |