Jordi Luong / Mbed 2 deprecated Project_BioRobotics

Project BioRobotics Group 19

Dependencies:   FastPWM HIDScope MODSERIAL QEI biquadFilter mbed

main.cpp@9:c19f6f0f5080, 2017-10-23 (annotated)

Committer:
jordiluong
Date:
Mon Oct 23 14:13:47 2017 +0000
Revision:
9:c19f6f0f5080
Parent:
6:246b05228f52 
Extended to two motors

Who changed what in which revision?

UserRevisionLine numberNew 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 9:c19f6f0f5080 37 InterruptIn button3(SW2);
jordiluong 9:c19f6f0f5080 38 InterruptIn button4(SW3);
jordiluong 9:c19f6f0f5080 39 AnalogIn pot1Pin(A0);
jordiluong 9:c19f6f0f5080 40 AnalogIn pot2Pin(A1);
jordiluong 3:5c3edcd29448 41
jordiluong 0:80ac024b84cb 42 // DEFINITIONS
jordiluong 6:246b05228f52 43 const float motorVelocity = 4.2; // unit: rad/s
jordiluong 4:ea7689bf97e1 44 const float motorGain = 8.4; // unit: (rad/s) / PWM
jordiluong 4:ea7689bf97e1 45 const int motorRatio = 131; // Ratio of the gearbox in the motors
jordiluong 4:ea7689bf97e1 46 int motor1State = 0; // 0: Off, 1: CW, 2: CCW
jordiluong 9:c19f6f0f5080 47 int motor2State = 0;
jordiluong 6:246b05228f52 48 float motorValue = 0;
jordiluong 4:ea7689bf97e1 49
jordiluong 4:ea7689bf97e1 50 //TICKERS
jordiluong 4:ea7689bf97e1 51 Ticker encoder;
jordiluong 9:c19f6f0f5080 52 Ticker checkMotor1State;
jordiluong 9:c19f6f0f5080 53 Ticker checkMotor2State;
jordiluong 0:80ac024b84cb 54
jordiluong 0:80ac024b84cb 55 // FUNCTIONS
jordiluong 4:ea7689bf97e1 56 void RotateMotor1CW()
jordiluong 0:80ac024b84cb 57 {
jordiluong 4:ea7689bf97e1 58 motor1DirectionPin = 1;
jordiluong 6:246b05228f52 59 //motor1MagnitudePin = fabs(motorVelocity / motorGain);
jordiluong 9:c19f6f0f5080 60 motorValue = pot1Pin;
jordiluong 6:246b05228f52 61 motor1MagnitudePin = motorValue;
jordiluong 6:246b05228f52 62 //pc.printf("Rotating motor 1 CW \r\n");
jordiluong 9:c19f6f0f5080 63 pc.printf("Motor 1 CW %f \r\n", motorValue);
jordiluong 0:80ac024b84cb 64 }
jordiluong 0:80ac024b84cb 65
jordiluong 4:ea7689bf97e1 66 void RotateMotor1CCW()
jordiluong 0:80ac024b84cb 67 {
jordiluong 4:ea7689bf97e1 68 motor1DirectionPin = 0;
jordiluong 6:246b05228f52 69 //motor1MagnitudePin = fabs(motorVelocity / motorGain);
jordiluong 6:246b05228f52 70 //pc.printf("Rotating motor 1 CounterCW \r\n");
jordiluong 9:c19f6f0f5080 71 motorValue = pot1Pin;
jordiluong 6:246b05228f52 72 motor1MagnitudePin = motorValue;
jordiluong 6:246b05228f52 73 //pc.printf("Rotating motor 1 CW \r\n");
jordiluong 9:c19f6f0f5080 74 pc.printf("Motor 1 CCW %f \r\n", motorValue);
jordiluong 0:80ac024b84cb 75 }
jordiluong 0:80ac024b84cb 76
jordiluong 4:ea7689bf97e1 77 void TurnMotor1Off()
jordiluong 0:80ac024b84cb 78 {
jordiluong 4:ea7689bf97e1 79 motor1MagnitudePin = 0;
jordiluong 9:c19f6f0f5080 80 pc.printf("Motor 1 off \r\n");
jordiluong 0:80ac024b84cb 81 }
jordiluong 0:80ac024b84cb 82
jordiluong 4:ea7689bf97e1 83 void CheckMotor1() // Checks the state of motor1 and rotates motor1 depending on the state
jordiluong 0:80ac024b84cb 84 {
jordiluong 4:ea7689bf97e1 85 switch(motor1State)
jordiluong 4:ea7689bf97e1 86 {
jordiluong 4:ea7689bf97e1 87 case 0: // Turn motors off
jordiluong 4:ea7689bf97e1 88 {TurnMotor1Off(); break;}
jordiluong 4:ea7689bf97e1 89
jordiluong 4:ea7689bf97e1 90 case 1: // Rotate motor 1 CW
jordiluong 4:ea7689bf97e1 91 {RotateMotor1CW(); break;}
jordiluong 4:ea7689bf97e1 92
jordiluong 4:ea7689bf97e1 93 case 2: // Rotate motor 2 CCW
jordiluong 4:ea7689bf97e1 94 {RotateMotor1CCW(); break;}
jordiluong 4:ea7689bf97e1 95
jordiluong 4:ea7689bf97e1 96 default:
jordiluong 4:ea7689bf97e1 97 break;
jordiluong 4:ea7689bf97e1 98 }
jordiluong 3:5c3edcd29448 99 }
jordiluong 9:c19f6f0f5080 100
jordiluong 9:c19f6f0f5080 101 void RotateMotor2CW()
jordiluong 9:c19f6f0f5080 102 {
jordiluong 9:c19f6f0f5080 103 motor2DirectionPin = 1;
jordiluong 9:c19f6f0f5080 104 //motor1MagnitudePin = fabs(motorVelocity / motorGain);
jordiluong 9:c19f6f0f5080 105 motorValue = pot2Pin;
jordiluong 9:c19f6f0f5080 106 motor2MagnitudePin = motorValue;
jordiluong 9:c19f6f0f5080 107 //pc.printf("Rotating motor 1 CW \r\n");
jordiluong 9:c19f6f0f5080 108 pc.printf("Motor 2 CW %f \r\n", motorValue);
jordiluong 9:c19f6f0f5080 109 }
jordiluong 9:c19f6f0f5080 110
jordiluong 9:c19f6f0f5080 111 void RotateMotor2CCW()
jordiluong 9:c19f6f0f5080 112 {
jordiluong 9:c19f6f0f5080 113 motor2DirectionPin = 0;
jordiluong 9:c19f6f0f5080 114 //motor1MagnitudePin = fabs(motorVelocity / motorGain);
jordiluong 9:c19f6f0f5080 115 //pc.printf("Rotating motor 1 CounterCW \r\n");
jordiluong 9:c19f6f0f5080 116 motorValue = pot2Pin;
jordiluong 9:c19f6f0f5080 117 motor2MagnitudePin = motorValue;
jordiluong 9:c19f6f0f5080 118 //pc.printf("Rotating motor 1 CW \r\n");
jordiluong 9:c19f6f0f5080 119 pc.printf("Motor 2 CCW %f \r\n", motorValue);
jordiluong 9:c19f6f0f5080 120 }
jordiluong 9:c19f6f0f5080 121
jordiluong 9:c19f6f0f5080 122 void TurnMotor2Off()
jordiluong 9:c19f6f0f5080 123 {
jordiluong 9:c19f6f0f5080 124 motor2MagnitudePin = 0;
jordiluong 9:c19f6f0f5080 125 pc.printf("Motor 2 off \r\n");
jordiluong 9:c19f6f0f5080 126 }
jordiluong 9:c19f6f0f5080 127
jordiluong 9:c19f6f0f5080 128 void CheckMotor2() // Checks the state of motor1 and rotates motor1 depending on the state
jordiluong 9:c19f6f0f5080 129 {
jordiluong 9:c19f6f0f5080 130 switch(motor2State)
jordiluong 9:c19f6f0f5080 131 {
jordiluong 9:c19f6f0f5080 132 case 0: // Turn motors off
jordiluong 9:c19f6f0f5080 133 {TurnMotor2Off(); break;}
jordiluong 9:c19f6f0f5080 134
jordiluong 9:c19f6f0f5080 135 case 1: // Rotate motor 1 CW
jordiluong 9:c19f6f0f5080 136 {RotateMotor2CW(); break;}
jordiluong 9:c19f6f0f5080 137
jordiluong 9:c19f6f0f5080 138 case 2: // Rotate motor 2 CCW
jordiluong 9:c19f6f0f5080 139 {RotateMotor2CCW(); break;}
jordiluong 9:c19f6f0f5080 140
jordiluong 9:c19f6f0f5080 141 default:
jordiluong 9:c19f6f0f5080 142 break;
jordiluong 9:c19f6f0f5080 143 }
jordiluong 9:c19f6f0f5080 144 }
jordiluong 6:246b05228f52 145 /*
jordiluong 4:ea7689bf97e1 146 void EncoderCalc()
jordiluong 3:5c3edcd29448 147 {
jordiluong 4:ea7689bf97e1 148 counts = Encoder.getPulses()/motorRatio;
jordiluong 4:ea7689bf97e1 149 degrees = counts/32*360;
jordiluong 4:ea7689bf97e1 150 omwentelingen = counts/32;
jordiluong 4:ea7689bf97e1 151 snelheid_tpm = (omwentelingen-vorig_omwentelingen)/0.5*60;
jordiluong 4:ea7689bf97e1 152 snelheid_tps = (omwentelingen-vorig_omwentelingen)/0.5;
jordiluong 4:ea7689bf97e1 153 snelheid_degps = (degrees-vorig_degrees)/0.5;
jordiluong 4:ea7689bf97e1 154 vorig_omwentelingen = omwentelingen;
jordiluong 4:ea7689bf97e1 155 vorig_degrees = degrees;
jordiluong 4:ea7689bf97e1 156 //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 157 }
jordiluong 6:246b05228f52 158 */
jordiluong 0:80ac024b84cb 159
jordiluong 0:80ac024b84cb 160 // States function
jordiluong 0:80ac024b84cb 161 void ProcessStateMachine()
jordiluong 0:80ac024b84cb 162 {
jordiluong 0:80ac024b84cb 163 switch(currentState)
jordiluong 0:80ac024b84cb 164 {
jordiluong 0:80ac024b84cb 165 case MOTORS_OFF:
jordiluong 0:80ac024b84cb 166 {
jordiluong 0:80ac024b84cb 167 // State initialization
jordiluong 0:80ac024b84cb 168 if(stateChanged)
jordiluong 0:80ac024b84cb 169 {
jordiluong 0:80ac024b84cb 170 pc.printf("Entering MOTORS_OFF \r\n");
jordiluong 4:ea7689bf97e1 171 TurnMotor1Off(); // Turn motors off
jordiluong 0:80ac024b84cb 172 stateChanged = false;
jordiluong 0:80ac024b84cb 173 }
jordiluong 0:80ac024b84cb 174
jordiluong 0:80ac024b84cb 175 // Home command
jordiluong 4:ea7689bf97e1 176 if(!button1)
jordiluong 0:80ac024b84cb 177 {
jordiluong 0:80ac024b84cb 178 currentState = MOVING;
jordiluong 0:80ac024b84cb 179 stateChanged = true;
jordiluong 0:80ac024b84cb 180 break;
jordiluong 0:80ac024b84cb 181 }
jordiluong 4:ea7689bf97e1 182 break;
jordiluong 0:80ac024b84cb 183 }
jordiluong 0:80ac024b84cb 184
jordiluong 0:80ac024b84cb 185 case MOVING:
jordiluong 0:80ac024b84cb 186 {
jordiluong 0:80ac024b84cb 187 // State initialization
jordiluong 0:80ac024b84cb 188 if(stateChanged)
jordiluong 0:80ac024b84cb 189 {
jordiluong 0:80ac024b84cb 190 pc.printf("Entering MOVING \r\n");
jordiluong 0:80ac024b84cb 191 stateChanged = false;
jordiluong 0:80ac024b84cb 192 }
jordiluong 4:ea7689bf97e1 193
jordiluong 4:ea7689bf97e1 194 // Move commands
jordiluong 0:80ac024b84cb 195
jordiluong 4:ea7689bf97e1 196 if(!button1){motor1State = 1;}
jordiluong 4:ea7689bf97e1 197 else if(!button2){motor1State = 2;}
jordiluong 4:ea7689bf97e1 198 else{motor1State = 0;}
jordiluong 0:80ac024b84cb 199
jordiluong 9:c19f6f0f5080 200 if(!button3){motor2State = 1;}
jordiluong 9:c19f6f0f5080 201 else if(!button4){motor2State = 2;}
jordiluong 9:c19f6f0f5080 202 else{motor2State = 0;}
jordiluong 9:c19f6f0f5080 203
jordiluong 4:ea7689bf97e1 204 /*
jordiluong 0:80ac024b84cb 205 // Hit command
jordiluong 2:d3687b2c4e37 206 if(// HIT COMMAND)
jordiluong 0:80ac024b84cb 207 {
jordiluong 0:80ac024b84cb 208 currentState = HITTING;
jordiluong 0:80ac024b84cb 209 stateChanged = true;
jordiluong 0:80ac024b84cb 210 break;
jordiluong 0:80ac024b84cb 211 }
jordiluong 3:5c3edcd29448 212 */
jordiluong 4:ea7689bf97e1 213 break;
jordiluong 0:80ac024b84cb 214 }
jordiluong 0:80ac024b84cb 215
jordiluong 0:80ac024b84cb 216 case HITTING:
jordiluong 0:80ac024b84cb 217 {
jordiluong 0:80ac024b84cb 218 // State initialization
jordiluong 0:80ac024b84cb 219 if(stateChanged)
jordiluong 0:80ac024b84cb 220 {
jordiluong 0:80ac024b84cb 221 pc.printf("Entering HITTING \r\n");
jordiluong 4:ea7689bf97e1 222 //HitBall(); // Hit the ball
jordiluong 0:80ac024b84cb 223 stateChanged = false;
jordiluong 0:80ac024b84cb 224 currentState = MOVING;
jordiluong 0:80ac024b84cb 225 stateChanged = true;
jordiluong 0:80ac024b84cb 226 break;
jordiluong 0:80ac024b84cb 227 }
jordiluong 4:ea7689bf97e1 228 break;
jordiluong 0:80ac024b84cb 229 }
jordiluong 0:80ac024b84cb 230
jordiluong 0:80ac024b84cb 231 default:
jordiluong 0:80ac024b84cb 232 {
jordiluong 4:ea7689bf97e1 233 TurnMotor1Off(); // Turn motors off for safety
jordiluong 9:c19f6f0f5080 234 TurnMotor2Off();
jordiluong 4:ea7689bf97e1 235 break;
jordiluong 0:80ac024b84cb 236 }
jordiluong 0:80ac024b84cb 237 }
jordiluong 0:80ac024b84cb 238 }
jordiluong 0:80ac024b84cb 239
jordiluong 0:80ac024b84cb 240 // Main function
jordiluong 0:80ac024b84cb 241 int main()
jordiluong 0:80ac024b84cb 242 {
jordiluong 0:80ac024b84cb 243 // Serial communication
jordiluong 0:80ac024b84cb 244 pc.baud(115200);
jordiluong 0:80ac024b84cb 245
jordiluong 4:ea7689bf97e1 246 pc.printf("START \r\n");
jordiluong 4:ea7689bf97e1 247
jordiluong 6:246b05228f52 248 //encoder.attach(EncoderCalc, 0.5);
jordiluong 9:c19f6f0f5080 249 checkMotor1State.attach(&CheckMotor1, 0.01);
jordiluong 9:c19f6f0f5080 250 checkMotor2State.attach(&CheckMotor2, 0.01);
jordiluong 4:ea7689bf97e1 251
jordiluong 0:80ac024b84cb 252 while(true)
jordiluong 0:80ac024b84cb 253 {
jordiluong 0:80ac024b84cb 254 ProcessStateMachine(); // Execute states function
jordiluong 0:80ac024b84cb 255 }
jordiluong 0:80ac024b84cb 256 }