Henk van Gansen / Mbed 2 deprecated Project_BioRobotics

Vanmorgen aan gewerkt

Dependencies:   FastPWM HIDScope MODSERIAL QEI biquadFilter mbed

Fork of Project_BioRobotics by Jordi Luong

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?

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 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 }