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