Fabrice Tshilumba / Mbed 2 deprecated state_program

Dependencies:   FastPWM MODSERIAL QEI biquadFilter mbed

Fork of state_program by Jesse Lohman

main.cpp@2:5cace74299e7, 2018-10-30 (annotated)

Committer:
JesseLohman
Date:
Tue Oct 30 09:15:02 2018 +0000
Revision:
2:5cace74299e7
Parent:
1:4cb9af313c26
Child:
3:be922ea2415f
Added condition for state switch from motor cal to check if velocity is 0

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JesseLohman 0:2a5dd6cc0008 1 #include "mbed.h"
JesseLohman 0:2a5dd6cc0008 2 #include "FastPWM.h"
JesseLohman 0:2a5dd6cc0008 3 #include "QEI.h"
JesseLohman 0:2a5dd6cc0008 4 #include "MODSERIAL.h"
JesseLohman 0:2a5dd6cc0008 5 #include "BiQuad.h"
JesseLohman 0:2a5dd6cc0008 6
JesseLohman 0:2a5dd6cc0008 7 enum States {WaitState, MotorCalState, EMGCalState, HomingState, MovingState, GrippingState, ScrewingState, FailureState};
JesseLohman 0:2a5dd6cc0008 8 States currentState = WaitState;
JesseLohman 0:2a5dd6cc0008 9
JesseLohman 0:2a5dd6cc0008 10 DigitalIn startButton(D0);
JesseLohman 0:2a5dd6cc0008 11 InterruptIn failureButton(D1);
JesseLohman 1:4cb9af313c26 12 DigitalIn grippingSwitch(SW2);
JesseLohman 1:4cb9af313c26 13 DigitalIn screwingSwitch(SW3);
JesseLohman 1:4cb9af313c26 14 DigitalIn gripDirection(D2);
JesseLohman 1:4cb9af313c26 15 DigitalIn screwDirection(D3);
JesseLohman 0:2a5dd6cc0008 16 MODSERIAL pc(USBTX, USBRX);
JesseLohman 1:4cb9af313c26 17 DigitalOut led1(LED1); // Red led
JesseLohman 1:4cb9af313c26 18 DigitalOut led2(LED2); // Green led
JesseLohman 1:4cb9af313c26 19 DigitalOut led3(LED3); // Blue led
JesseLohman 2:5cace74299e7 20 QEI encoder1(D14, D15, NC, 8400, QEI::X4_ENCODING);
JesseLohman 2:5cace74299e7 21 //QEI encoder2(A2, A3), NC, 4200);
JesseLohman 2:5cace74299e7 22 //QEI encoder3(A4, A5), NC, 4200);
JesseLohman 2:5cace74299e7 23 AnalogIn pot(A0); // Speed knob
JesseLohman 0:2a5dd6cc0008 24
JesseLohman 0:2a5dd6cc0008 25 bool stateChanged = true;
JesseLohman 0:2a5dd6cc0008 26
JesseLohman 0:2a5dd6cc0008 27 Ticker mainTicker;
JesseLohman 0:2a5dd6cc0008 28 Timer stateTimer;
JesseLohman 2:5cace74299e7 29
JesseLohman 2:5cace74299e7 30 const double sampleTime = 0.001;
JesseLohman 2:5cace74299e7 31 const float maxVelocity=8.4; // in rad/s
JesseLohman 2:5cace74299e7 32 const double PI = 3.141592653589793238463;
JesseLohman 2:5cace74299e7 33
JesseLohman 2:5cace74299e7 34 double u1, u2, u3, u4; // u1 is motor output of the long link, u2 is motor of the short link, u3 is motor of gripper, u4 is motor of screwer
JesseLohman 2:5cace74299e7 35 FastPWM pwmpin1(D5); //motor pwm
JesseLohman 2:5cace74299e7 36 DigitalOut directionpin1(D4); // motor direction
JesseLohman 2:5cace74299e7 37 double robotEndPoint;
JesseLohman 2:5cace74299e7 38
JesseLohman 2:5cace74299e7 39 double v;
JesseLohman 2:5cace74299e7 40 double Dpulses;
JesseLohman 0:2a5dd6cc0008 41
JesseLohman 0:2a5dd6cc0008 42 void switchToFailureState ()
JesseLohman 0:2a5dd6cc0008 43 {
JesseLohman 0:2a5dd6cc0008 44 failureButton.fall(NULL); // Detaches button, so it can only be activated once
JesseLohman 1:4cb9af313c26 45 led1 = 0;
JesseLohman 1:4cb9af313c26 46 led2 = 1;
JesseLohman 1:4cb9af313c26 47 led3 = 1;
JesseLohman 1:4cb9af313c26 48 pc.printf("SYSTEM FAILED\n");
JesseLohman 0:2a5dd6cc0008 49 currentState = FailureState;
JesseLohman 0:2a5dd6cc0008 50 stateChanged = true;
JesseLohman 0:2a5dd6cc0008 51 }
JesseLohman 0:2a5dd6cc0008 52
JesseLohman 2:5cace74299e7 53 double measureVelocity (int motor)
JesseLohman 2:5cace74299e7 54 {
JesseLohman 2:5cace74299e7 55 static double lastPulses = 0;
JesseLohman 2:5cace74299e7 56 double currentPulses;
JesseLohman 2:5cace74299e7 57 static double velocity = 0;
JesseLohman 2:5cace74299e7 58
JesseLohman 2:5cace74299e7 59 static int i = 0;
JesseLohman 2:5cace74299e7 60 if (i == 10) { // Encoder is not accurate enough, so with 1000 Hz the velocity can only be 0, 1000, 2000 or 3000 pulses/s
JesseLohman 2:5cace74299e7 61 switch (motor) { // Check which motor to measure
JesseLohman 2:5cace74299e7 62 case 1:
JesseLohman 2:5cace74299e7 63 currentPulses = encoder1.getPulses();
JesseLohman 2:5cace74299e7 64 break;
JesseLohman 2:5cace74299e7 65 case 2:
JesseLohman 2:5cace74299e7 66 //currentPulses = encoder2.getPulses();
JesseLohman 2:5cace74299e7 67 break;
JesseLohman 2:5cace74299e7 68 case 3:
JesseLohman 2:5cace74299e7 69 //currentPulses = encoder3.getPulses();
JesseLohman 2:5cace74299e7 70 break;
JesseLohman 2:5cace74299e7 71 }
JesseLohman 2:5cace74299e7 72
JesseLohman 2:5cace74299e7 73 double deltaPulses = currentPulses - lastPulses;
JesseLohman 2:5cace74299e7 74 Dpulses = deltaPulses;
JesseLohman 2:5cace74299e7 75 velocity = deltaPulses / (sampleTime * 10); // Velocity in pulses/s
JesseLohman 2:5cace74299e7 76 lastPulses = currentPulses;
JesseLohman 2:5cace74299e7 77 i = 0;
JesseLohman 2:5cace74299e7 78 } else {
JesseLohman 2:5cace74299e7 79 i += 1;
JesseLohman 2:5cace74299e7 80 }
JesseLohman 2:5cace74299e7 81 v = velocity;
JesseLohman 2:5cace74299e7 82 return velocity;
JesseLohman 2:5cace74299e7 83 }
JesseLohman 2:5cace74299e7 84
JesseLohman 2:5cace74299e7 85 void measureAll ()
JesseLohman 2:5cace74299e7 86 {
JesseLohman 2:5cace74299e7 87
JesseLohman 2:5cace74299e7 88 }
JesseLohman 2:5cace74299e7 89
JesseLohman 2:5cace74299e7 90 void getMotorControlSignal () // Milestone 1 code, not relevant anymore
JesseLohman 2:5cace74299e7 91 {
JesseLohman 2:5cace74299e7 92 double potSignal = pot.read(); // read pot and scale to motor control signal
JesseLohman 2:5cace74299e7 93 //pc.printf("motor control signal = %f\n", potSignal);
JesseLohman 2:5cace74299e7 94 u1 = potSignal;
JesseLohman 2:5cace74299e7 95 }
JesseLohman 2:5cace74299e7 96
JesseLohman 2:5cace74299e7 97 void controlMotor () // Control direction and speed
JesseLohman 2:5cace74299e7 98 {
JesseLohman 2:5cace74299e7 99 directionpin1 = u1 > 0.0f; // Either true or false
JesseLohman 2:5cace74299e7 100 pwmpin1 = fabs(u1);
JesseLohman 2:5cace74299e7 101 }
JesseLohman 2:5cace74299e7 102
JesseLohman 0:2a5dd6cc0008 103 void stateMachine ()
JesseLohman 0:2a5dd6cc0008 104 {
JesseLohman 0:2a5dd6cc0008 105 switch (currentState) {
JesseLohman 0:2a5dd6cc0008 106 case WaitState:
JesseLohman 0:2a5dd6cc0008 107 if (stateChanged == true) {
JesseLohman 1:4cb9af313c26 108 led1 = 0;
JesseLohman 1:4cb9af313c26 109 led2 = 1;
JesseLohman 1:4cb9af313c26 110 led3 = 1;
JesseLohman 0:2a5dd6cc0008 111 // Entry action: all the things you do once in this state
JesseLohman 2:5cace74299e7 112 u1 = 0; // Turn off all motors
JesseLohman 2:5cace74299e7 113 u2 = 0;
JesseLohman 2:5cace74299e7 114 u3 = 0;
JesseLohman 2:5cace74299e7 115 u4 = 0;
JesseLohman 0:2a5dd6cc0008 116 stateChanged = false;
JesseLohman 0:2a5dd6cc0008 117 }
JesseLohman 0:2a5dd6cc0008 118
JesseLohman 2:5cace74299e7 119 if (startButton.read() == false) { // When button is pressed, value is false
JesseLohman 1:4cb9af313c26 120 //pc.printf("Switching to motor calibration");
JesseLohman 1:4cb9af313c26 121 led1 = 1;
JesseLohman 0:2a5dd6cc0008 122 currentState = MotorCalState;
JesseLohman 0:2a5dd6cc0008 123 stateChanged = true;
JesseLohman 0:2a5dd6cc0008 124 }
JesseLohman 0:2a5dd6cc0008 125
JesseLohman 0:2a5dd6cc0008 126 break;
JesseLohman 0:2a5dd6cc0008 127 case MotorCalState:
JesseLohman 0:2a5dd6cc0008 128 if (stateChanged == true) {
JesseLohman 0:2a5dd6cc0008 129 // Entry action: all the things you do once in this state
JesseLohman 1:4cb9af313c26 130 led2 = 0;
JesseLohman 0:2a5dd6cc0008 131 // Set motorpwm to 'low' value
JesseLohman 2:5cace74299e7 132 u1 = 0.6; //TODO: Check if direction is right
JesseLohman 2:5cace74299e7 133 u2 = 0.6;
JesseLohman 0:2a5dd6cc0008 134 stateTimer.reset();
JesseLohman 0:2a5dd6cc0008 135 stateTimer.start();
JesseLohman 0:2a5dd6cc0008 136 stateChanged = false;
JesseLohman 0:2a5dd6cc0008 137 }
JesseLohman 0:2a5dd6cc0008 138
JesseLohman 0:2a5dd6cc0008 139 // Add stuff you do every loop
JesseLohman 2:5cace74299e7 140 getMotorControlSignal();
JesseLohman 0:2a5dd6cc0008 141
JesseLohman 2:5cace74299e7 142 if (stateTimer >= 3.0f && fabs(measureVelocity(1)) < 100 && screwingSwitch.read() == false) { //TODO: add && fabs(measureVelocity(2)) < 0.1f
JesseLohman 2:5cace74299e7 143 //TODO: Add reset of encoder2
JesseLohman 1:4cb9af313c26 144 led2 = 1;
JesseLohman 2:5cace74299e7 145 encoder1.reset(); // Reset encoder for the 0 position
JesseLohman 0:2a5dd6cc0008 146 currentState = EMGCalState;
JesseLohman 0:2a5dd6cc0008 147 stateChanged = true;
JesseLohman 2:5cace74299e7 148 u1 = 0; // Turn off motors
JesseLohman 2:5cace74299e7 149 u2 = 0;
JesseLohman 0:2a5dd6cc0008 150 }
JesseLohman 0:2a5dd6cc0008 151 break;
JesseLohman 0:2a5dd6cc0008 152 case EMGCalState:
JesseLohman 0:2a5dd6cc0008 153 if (stateChanged == true) {
JesseLohman 0:2a5dd6cc0008 154 // Entry action: all the things you do once in this state;
JesseLohman 1:4cb9af313c26 155 led3 = 0;
JesseLohman 0:2a5dd6cc0008 156 stateTimer.reset();
JesseLohman 0:2a5dd6cc0008 157 stateTimer.start();
JesseLohman 0:2a5dd6cc0008 158 stateChanged = false;
JesseLohman 0:2a5dd6cc0008 159 }
JesseLohman 0:2a5dd6cc0008 160
JesseLohman 0:2a5dd6cc0008 161 // Add stuff you do every loop
JesseLohman 0:2a5dd6cc0008 162
JesseLohman 0:2a5dd6cc0008 163 if (stateTimer >= 3.0f) {
JesseLohman 1:4cb9af313c26 164 //pc.printf("Starting homing...\n");
JesseLohman 1:4cb9af313c26 165 led3 = 1;
JesseLohman 0:2a5dd6cc0008 166 currentState = HomingState;
JesseLohman 0:2a5dd6cc0008 167 stateChanged = true;
JesseLohman 0:2a5dd6cc0008 168 }
JesseLohman 0:2a5dd6cc0008 169 break;
JesseLohman 0:2a5dd6cc0008 170 case HomingState:
JesseLohman 0:2a5dd6cc0008 171 if (stateChanged == true) {
JesseLohman 0:2a5dd6cc0008 172 // Entry action: all the things you do once in this state;
JesseLohman 1:4cb9af313c26 173 led1 = 0;
JesseLohman 1:4cb9af313c26 174 led2 = 0; // Emits yellow together
JesseLohman 1:4cb9af313c26 175 //TODO: Set intended position
JesseLohman 0:2a5dd6cc0008 176 stateChanged = false;
JesseLohman 0:2a5dd6cc0008 177 }
JesseLohman 0:2a5dd6cc0008 178
JesseLohman 0:2a5dd6cc0008 179 // Nothing extra happens till robot reaches starting position and button is pressed
JesseLohman 0:2a5dd6cc0008 180
JesseLohman 1:4cb9af313c26 181 if (startButton.read() == false) { //TODO: Also add position condition
JesseLohman 1:4cb9af313c26 182 led1 = 1;
JesseLohman 1:4cb9af313c26 183 led2 = 1;
JesseLohman 0:2a5dd6cc0008 184 currentState = MovingState;
JesseLohman 0:2a5dd6cc0008 185 stateChanged = true;
JesseLohman 0:2a5dd6cc0008 186 }
JesseLohman 0:2a5dd6cc0008 187 break;
JesseLohman 0:2a5dd6cc0008 188 case MovingState:
JesseLohman 1:4cb9af313c26 189 if (stateChanged == true) {
JesseLohman 0:2a5dd6cc0008 190 // Entry action: all the things you do once in this state;
JesseLohman 1:4cb9af313c26 191 led1 = 0;
JesseLohman 1:4cb9af313c26 192 led2 = 0;
JesseLohman 1:4cb9af313c26 193 led3 = 0; // Emits white together
JesseLohman 1:4cb9af313c26 194 stateChanged = false;
JesseLohman 1:4cb9af313c26 195 }
JesseLohman 1:4cb9af313c26 196
JesseLohman 1:4cb9af313c26 197 if (grippingSwitch.read() == false) {
JesseLohman 0:2a5dd6cc0008 198 led1 = 1;
JesseLohman 0:2a5dd6cc0008 199 led2 = 1;
JesseLohman 0:2a5dd6cc0008 200 led3 = 1;
JesseLohman 1:4cb9af313c26 201 currentState = GrippingState;
JesseLohman 1:4cb9af313c26 202 stateChanged = true;
JesseLohman 1:4cb9af313c26 203 }
JesseLohman 1:4cb9af313c26 204
JesseLohman 1:4cb9af313c26 205 break;
JesseLohman 1:4cb9af313c26 206 case GrippingState:
JesseLohman 1:4cb9af313c26 207 if (stateChanged == true) {
JesseLohman 1:4cb9af313c26 208 // Entry action: all the things you do once in this state;
JesseLohman 1:4cb9af313c26 209 led2 = 0;
JesseLohman 2:5cace74299e7 210 led3 = 0; // Emits light blue together
JesseLohman 0:2a5dd6cc0008 211 stateChanged = false;
JesseLohman 0:2a5dd6cc0008 212 }
JesseLohman 1:4cb9af313c26 213
JesseLohman 1:4cb9af313c26 214 if (gripDirection == true) {
JesseLohman 1:4cb9af313c26 215 // Close gripper
JesseLohman 1:4cb9af313c26 216 } else {
JesseLohman 1:4cb9af313c26 217 // Open gripper
JesseLohman 1:4cb9af313c26 218 }
JesseLohman 1:4cb9af313c26 219
JesseLohman 1:4cb9af313c26 220 if (screwingSwitch.read() == false) {
JesseLohman 1:4cb9af313c26 221 led2 = 1;
JesseLohman 1:4cb9af313c26 222 led3 = 1;
JesseLohman 1:4cb9af313c26 223 currentState = ScrewingState;
JesseLohman 1:4cb9af313c26 224 stateChanged = true;
JesseLohman 1:4cb9af313c26 225 }
JesseLohman 1:4cb9af313c26 226 if (startButton.read() == false) {
JesseLohman 1:4cb9af313c26 227 led2 = 1;
JesseLohman 1:4cb9af313c26 228 led3 = 1;
JesseLohman 1:4cb9af313c26 229 currentState = MovingState;
JesseLohman 1:4cb9af313c26 230 stateChanged = true;
JesseLohman 1:4cb9af313c26 231 }
JesseLohman 0:2a5dd6cc0008 232 break;
JesseLohman 0:2a5dd6cc0008 233 case ScrewingState:
JesseLohman 1:4cb9af313c26 234 if (stateChanged == true) {
JesseLohman 1:4cb9af313c26 235 // Entry action: all the things you do once in this state;
JesseLohman 1:4cb9af313c26 236 led1 = 0;
JesseLohman 1:4cb9af313c26 237 led3 = 0; // Emits pink together
JesseLohman 1:4cb9af313c26 238 stateChanged = false;
JesseLohman 1:4cb9af313c26 239 }
JesseLohman 2:5cace74299e7 240
JesseLohman 1:4cb9af313c26 241 if (screwDirection == true) {
JesseLohman 1:4cb9af313c26 242 // Screw
JesseLohman 1:4cb9af313c26 243 } else {
JesseLohman 1:4cb9af313c26 244 // Unscrew
JesseLohman 1:4cb9af313c26 245 }
JesseLohman 2:5cace74299e7 246
JesseLohman 1:4cb9af313c26 247 if (startButton.read() == false) {
JesseLohman 1:4cb9af313c26 248 led1 = 1;
JesseLohman 1:4cb9af313c26 249 led3 = 1;
JesseLohman 1:4cb9af313c26 250 currentState = MovingState;
JesseLohman 1:4cb9af313c26 251 stateChanged = true;
JesseLohman 1:4cb9af313c26 252 }
JesseLohman 0:2a5dd6cc0008 253 break;
JesseLohman 0:2a5dd6cc0008 254 case FailureState:
JesseLohman 0:2a5dd6cc0008 255 if (stateChanged == true) {
JesseLohman 0:2a5dd6cc0008 256 // Entry action: all the things you do once in this state
JesseLohman 2:5cace74299e7 257 u1 = 0; // Turn off all motors
JesseLohman 2:5cace74299e7 258 u2 = 0;
JesseLohman 2:5cace74299e7 259 u3 = 0;
JesseLohman 2:5cace74299e7 260 u4 = 0;
JesseLohman 0:2a5dd6cc0008 261 stateChanged = false;
JesseLohman 0:2a5dd6cc0008 262 }
JesseLohman 1:4cb9af313c26 263
JesseLohman 1:4cb9af313c26 264 static double blinkTimer = 0;
JesseLohman 1:4cb9af313c26 265 if (blinkTimer >= 0.5) {
JesseLohman 1:4cb9af313c26 266 led1 = !led1;
JesseLohman 1:4cb9af313c26 267 blinkTimer = 0;
JesseLohman 1:4cb9af313c26 268 }
JesseLohman 1:4cb9af313c26 269 blinkTimer += sampleTime;
JesseLohman 1:4cb9af313c26 270
JesseLohman 1:4cb9af313c26 271 break;
JesseLohman 0:2a5dd6cc0008 272 }
JesseLohman 0:2a5dd6cc0008 273 }
JesseLohman 0:2a5dd6cc0008 274
JesseLohman 0:2a5dd6cc0008 275 void mainLoop ()
JesseLohman 0:2a5dd6cc0008 276 {
JesseLohman 0:2a5dd6cc0008 277 // Add measure, motor controller and output function
JesseLohman 2:5cace74299e7 278 measureAll();
JesseLohman 0:2a5dd6cc0008 279 stateMachine();
JesseLohman 2:5cace74299e7 280 controlMotor();
JesseLohman 0:2a5dd6cc0008 281 }
JesseLohman 0:2a5dd6cc0008 282
JesseLohman 0:2a5dd6cc0008 283 int main()
JesseLohman 0:2a5dd6cc0008 284 {
JesseLohman 2:5cace74299e7 285 pc.printf("checkpoint 1\n");
JesseLohman 0:2a5dd6cc0008 286 pc.baud(115200);
JesseLohman 0:2a5dd6cc0008 287 mainTicker.attach(mainLoop, sampleTime);
JesseLohman 0:2a5dd6cc0008 288 failureButton.fall(&switchToFailureState); // When button is pressed FailureState is activated
JesseLohman 0:2a5dd6cc0008 289
JesseLohman 0:2a5dd6cc0008 290 while (true) {
JesseLohman 2:5cace74299e7 291 //pc.printf("State = %i\n", currentState);
JesseLohman 2:5cace74299e7 292 //int pulses = encoder1.getPulses();
JesseLohman 2:5cace74299e7 293 //pc.printf("pulses = %i\n", pulses);
JesseLohman 2:5cace74299e7 294 pc.printf("v = %f\n", v);
JesseLohman 2:5cace74299e7 295 pc.printf("delta pulses = %f\n", Dpulses);
JesseLohman 0:2a5dd6cc0008 296 wait(1);
JesseLohman 0:2a5dd6cc0008 297 }
JesseLohman 0:2a5dd6cc0008 298 }