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@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?

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