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@1:1221419474b3, 2017-09-25 (annotated)

Committer:
jordiluong
Date:
Mon Sep 25 14:29:28 2017 +0000
Revision:
1:1221419474b3
Parent:
0:80ac024b84cb
Child:
2:d3687b2c4e37
TEST VERSION

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jordiluong 1:1221419474b3 1 // TEST VERSION
jordiluong 0:80ac024b84cb 2 #include "BiQuad.h"
jordiluong 0:80ac024b84cb 3 #include "FastPWM.h"
jordiluong 0:80ac024b84cb 4 #include "HIDScope.h"
jordiluong 0:80ac024b84cb 5 #include "MODSERIAL.h"
jordiluong 0:80ac024b84cb 6 #include "mbed.h"
jordiluong 0:80ac024b84cb 7 #include "QEI.h"
jordiluong 0:80ac024b84cb 8
jordiluong 0:80ac024b84cb 9 // SERIAL COMMUNICATION WITH PC
jordiluong 0:80ac024b84cb 10 MODSERIAL pc(USBTX, USBRX);
jordiluong 0:80ac024b84cb 11
jordiluong 0:80ac024b84cb 12 // STATES
jordiluong 0:80ac024b84cb 13 enum states{MOTORS_OFF, HOMING, MOVING, HITTING};
jordiluong 0:80ac024b84cb 14
jordiluong 0:80ac024b84cb 15 states currentState = MOTORS_OFF; // Start with motors off
jordiluong 0:80ac024b84cb 16 bool stateChanged = true; // Make sure the initialization of first state is executed
jordiluong 0:80ac024b84cb 17
jordiluong 0:80ac024b84cb 18 // DEFINITIONS
jordiluong 1:1221419474b3 19 InterruptIn homeButton(SW2); // Button to go to home state
jordiluong 1:1221419474b3 20 InterruptIn hitButton(SW3); // Button to go to hit state
jordiluong 0:80ac024b84cb 21
jordiluong 1:1221419474b3 22 float voltage;
jordiluong 0:80ac024b84cb 23
jordiluong 0:80ac024b84cb 24 // FUNCTIONS
jordiluong 0:80ac024b84cb 25 // Turn motors off
jordiluong 0:80ac024b84cb 26 void TurnMotorsOff()
jordiluong 0:80ac024b84cb 27 {
jordiluong 1:1221419474b3 28 pc.printf("Turning motors off \r\n"); // Turn motors off
jordiluong 0:80ac024b84cb 29 }
jordiluong 0:80ac024b84cb 30
jordiluong 0:80ac024b84cb 31 // Move to home
jordiluong 0:80ac024b84cb 32 void MoveToHome()
jordiluong 0:80ac024b84cb 33 {
jordiluong 1:1221419474b3 34 pc.printf("Moving to home \r\n"); // Move to home
jordiluong 0:80ac024b84cb 35 }
jordiluong 0:80ac024b84cb 36
jordiluong 0:80ac024b84cb 37 // Filter signals
jordiluong 1:1221419474b3 38 float FilterSignal(float signal)
jordiluong 0:80ac024b84cb 39 {
jordiluong 1:1221419474b3 40 pc.printf("Filtering signal \r\n"); // Filter signal
jordiluong 1:1221419474b3 41 voltage = signal;
jordiluong 1:1221419474b3 42 return voltage;
jordiluong 0:80ac024b84cb 43 }
jordiluong 0:80ac024b84cb 44
jordiluong 0:80ac024b84cb 45 // Motor 1
jordiluong 1:1221419474b3 46 void RotateMotor1(float voltage)
jordiluong 0:80ac024b84cb 47 {
jordiluong 1:1221419474b3 48 pc.printf("Rotating motor 1 \r\n"); // Rotate motor 1
jordiluong 0:80ac024b84cb 49 }
jordiluong 0:80ac024b84cb 50
jordiluong 0:80ac024b84cb 51 // Motor 2
jordiluong 1:1221419474b3 52 void RotateMotor2(float voltage)
jordiluong 0:80ac024b84cb 53 {
jordiluong 1:1221419474b3 54 pc.printf("Rotating motor 2 \r\n"); // Rotate motor 2
jordiluong 0:80ac024b84cb 55 }
jordiluong 0:80ac024b84cb 56
jordiluong 0:80ac024b84cb 57 // Hit the ball
jordiluong 0:80ac024b84cb 58 void HitBall()
jordiluong 0:80ac024b84cb 59 {
jordiluong 1:1221419474b3 60 pc.printf("Hitting the ball \r\n"); // Rotate motor 3
jordiluong 0:80ac024b84cb 61 }
jordiluong 0:80ac024b84cb 62
jordiluong 0:80ac024b84cb 63 // States function
jordiluong 0:80ac024b84cb 64 void ProcessStateMachine()
jordiluong 0:80ac024b84cb 65 {
jordiluong 0:80ac024b84cb 66 switch(currentState)
jordiluong 0:80ac024b84cb 67 {
jordiluong 0:80ac024b84cb 68 case MOTORS_OFF:
jordiluong 0:80ac024b84cb 69 {
jordiluong 0:80ac024b84cb 70 // State initialization
jordiluong 0:80ac024b84cb 71 if(stateChanged)
jordiluong 0:80ac024b84cb 72 {
jordiluong 0:80ac024b84cb 73 pc.printf("Entering MOTORS_OFF \r\n");
jordiluong 0:80ac024b84cb 74 TurnMotorsOff(); // Turn motors off
jordiluong 0:80ac024b84cb 75 stateChanged = false;
jordiluong 0:80ac024b84cb 76 }
jordiluong 0:80ac024b84cb 77
jordiluong 0:80ac024b84cb 78 // Home command
jordiluong 1:1221419474b3 79 if(!homeButton)
jordiluong 0:80ac024b84cb 80 {
jordiluong 0:80ac024b84cb 81 currentState = HOMING;
jordiluong 0:80ac024b84cb 82 stateChanged = true;
jordiluong 0:80ac024b84cb 83 break;
jordiluong 0:80ac024b84cb 84 }
jordiluong 0:80ac024b84cb 85 }
jordiluong 0:80ac024b84cb 86
jordiluong 0:80ac024b84cb 87 case HOMING:
jordiluong 0:80ac024b84cb 88 {
jordiluong 0:80ac024b84cb 89 // State initialization
jordiluong 0:80ac024b84cb 90 if(stateChanged)
jordiluong 0:80ac024b84cb 91 {
jordiluong 0:80ac024b84cb 92 pc.printf("Entering HOMING \r\n");
jordiluong 0:80ac024b84cb 93 MoveToHome(); // Move to home position
jordiluong 0:80ac024b84cb 94 stateChanged = false;
jordiluong 0:80ac024b84cb 95 currentState = MOVING;
jordiluong 0:80ac024b84cb 96 stateChanged = true;
jordiluong 0:80ac024b84cb 97 break;
jordiluong 0:80ac024b84cb 98 }
jordiluong 0:80ac024b84cb 99 }
jordiluong 0:80ac024b84cb 100
jordiluong 0:80ac024b84cb 101 case MOVING:
jordiluong 0:80ac024b84cb 102 {
jordiluong 0:80ac024b84cb 103 // State initialization
jordiluong 0:80ac024b84cb 104 if(stateChanged)
jordiluong 0:80ac024b84cb 105 {
jordiluong 0:80ac024b84cb 106 pc.printf("Entering MOVING \r\n");
jordiluong 0:80ac024b84cb 107 stateChanged = false;
jordiluong 0:80ac024b84cb 108 }
jordiluong 0:80ac024b84cb 109
jordiluong 0:80ac024b84cb 110 // EMG signals to rotate motor 1
jordiluong 1:1221419474b3 111 if(false)
jordiluong 0:80ac024b84cb 112 {
jordiluong 1:1221419474b3 113 FilterSignal(1); // Filter the signal
jordiluong 1:1221419474b3 114 RotateMotor1(voltage); // Rotate motor 1
jordiluong 1:1221419474b3 115 pc.printf("%f \r\n", voltage);
jordiluong 0:80ac024b84cb 116 }
jordiluong 0:80ac024b84cb 117
jordiluong 0:80ac024b84cb 118 // EMG signals to rotate motor 2
jordiluong 1:1221419474b3 119 if(false)
jordiluong 0:80ac024b84cb 120 {
jordiluong 1:1221419474b3 121 FilterSignal(2); // Filter the signal
jordiluong 1:1221419474b3 122 RotateMotor2(voltage); // Rotate motor 2
jordiluong 1:1221419474b3 123 pc.printf("%f \r\n", voltage);
jordiluong 0:80ac024b84cb 124 }
jordiluong 0:80ac024b84cb 125
jordiluong 0:80ac024b84cb 126 // Hit command
jordiluong 1:1221419474b3 127 if(!hitButton)
jordiluong 0:80ac024b84cb 128 {
jordiluong 0:80ac024b84cb 129 currentState = HITTING;
jordiluong 0:80ac024b84cb 130 stateChanged = true;
jordiluong 0:80ac024b84cb 131 break;
jordiluong 0:80ac024b84cb 132 }
jordiluong 0:80ac024b84cb 133 }
jordiluong 0:80ac024b84cb 134
jordiluong 0:80ac024b84cb 135 case HITTING:
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 HITTING \r\n");
jordiluong 0:80ac024b84cb 141 HitBall(); // Hit the ball
jordiluong 0:80ac024b84cb 142 stateChanged = false;
jordiluong 0:80ac024b84cb 143 currentState = MOVING;
jordiluong 0:80ac024b84cb 144 stateChanged = true;
jordiluong 0:80ac024b84cb 145 break;
jordiluong 0:80ac024b84cb 146 }
jordiluong 0:80ac024b84cb 147 }
jordiluong 0:80ac024b84cb 148
jordiluong 0:80ac024b84cb 149 default:
jordiluong 0:80ac024b84cb 150 {
jordiluong 0:80ac024b84cb 151 TurnMotorsOff(); // Turn motors off for safety
jordiluong 0:80ac024b84cb 152 }
jordiluong 0:80ac024b84cb 153 }
jordiluong 0:80ac024b84cb 154 }
jordiluong 0:80ac024b84cb 155
jordiluong 0:80ac024b84cb 156 // Main function
jordiluong 0:80ac024b84cb 157 int main()
jordiluong 0:80ac024b84cb 158 {
jordiluong 0:80ac024b84cb 159 // Serial communication
jordiluong 0:80ac024b84cb 160 pc.baud(115200);
jordiluong 0:80ac024b84cb 161
jordiluong 0:80ac024b84cb 162 while(true)
jordiluong 0:80ac024b84cb 163 {
jordiluong 0:80ac024b84cb 164 ProcessStateMachine(); // Execute states function
jordiluong 0:80ac024b84cb 165 }
jordiluong 0:80ac024b84cb 166 }