Jordi Luong / Mbed 2 deprecated Project_BioRobotics

Project BioRobotics Group 19

Dependencies:   FastPWM HIDScope MODSERIAL QEI biquadFilter mbed

main.cpp@2:d3687b2c4e37, 2017-09-25 (annotated)

Committer:
jordiluong
Date:
Mon Sep 25 14:32:19 2017 +0000
Revision:
2:d3687b2c4e37
Parent:
1:1221419474b3
Child:
3:5c3edcd29448
First basic set-up of program

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 0:80ac024b84cb 12 enum states{MOTORS_OFF, HOMING, 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 0:80ac024b84cb 17 // DEFINITIONS
jordiluong 0:80ac024b84cb 18
jordiluong 0:80ac024b84cb 19
jordiluong 0:80ac024b84cb 20 // FUNCTIONS
jordiluong 0:80ac024b84cb 21 // Turn motors off
jordiluong 0:80ac024b84cb 22 void TurnMotorsOff()
jordiluong 0:80ac024b84cb 23 {
jordiluong 2:d3687b2c4e37 24 // Turn motors off
jordiluong 0:80ac024b84cb 25 }
jordiluong 0:80ac024b84cb 26
jordiluong 0:80ac024b84cb 27 // Move to home
jordiluong 0:80ac024b84cb 28 void MoveToHome()
jordiluong 0:80ac024b84cb 29 {
jordiluong 2:d3687b2c4e37 30 // Move to home
jordiluong 0:80ac024b84cb 31 }
jordiluong 0:80ac024b84cb 32
jordiluong 0:80ac024b84cb 33 // Filter signals
jordiluong 2:d3687b2c4e37 34 float FilterSignal(// Signal)
jordiluong 0:80ac024b84cb 35 {
jordiluong 2:d3687b2c4e37 36 // Filter signal
jordiluong 2:d3687b2c4e37 37 return // Voltage
jordiluong 0:80ac024b84cb 38 }
jordiluong 0:80ac024b84cb 39
jordiluong 0:80ac024b84cb 40 // Motor 1
jordiluong 2:d3687b2c4e37 41 void RotateMotor1(// Voltage)
jordiluong 0:80ac024b84cb 42 {
jordiluong 2:d3687b2c4e37 43 // Rotate motor 1
jordiluong 0:80ac024b84cb 44 }
jordiluong 0:80ac024b84cb 45
jordiluong 0:80ac024b84cb 46 // Motor 2
jordiluong 2:d3687b2c4e37 47 void RotateMotor2(// Voltage)
jordiluong 0:80ac024b84cb 48 {
jordiluong 2:d3687b2c4e37 49 // Rotate motor 2
jordiluong 0:80ac024b84cb 50 }
jordiluong 0:80ac024b84cb 51
jordiluong 0:80ac024b84cb 52 // Hit the ball
jordiluong 0:80ac024b84cb 53 void HitBall()
jordiluong 0:80ac024b84cb 54 {
jordiluong 2:d3687b2c4e37 55 // Rotate motor 3
jordiluong 0:80ac024b84cb 56 }
jordiluong 0:80ac024b84cb 57
jordiluong 0:80ac024b84cb 58 // States function
jordiluong 0:80ac024b84cb 59 void ProcessStateMachine()
jordiluong 0:80ac024b84cb 60 {
jordiluong 0:80ac024b84cb 61 switch(currentState)
jordiluong 0:80ac024b84cb 62 {
jordiluong 0:80ac024b84cb 63 case MOTORS_OFF:
jordiluong 0:80ac024b84cb 64 {
jordiluong 0:80ac024b84cb 65 // State initialization
jordiluong 0:80ac024b84cb 66 if(stateChanged)
jordiluong 0:80ac024b84cb 67 {
jordiluong 0:80ac024b84cb 68 pc.printf("Entering MOTORS_OFF \r\n");
jordiluong 0:80ac024b84cb 69 TurnMotorsOff(); // Turn motors off
jordiluong 0:80ac024b84cb 70 stateChanged = false;
jordiluong 0:80ac024b84cb 71 }
jordiluong 0:80ac024b84cb 72
jordiluong 0:80ac024b84cb 73 // Home command
jordiluong 2:d3687b2c4e37 74 if(//HOME COMMAND)
jordiluong 0:80ac024b84cb 75 {
jordiluong 0:80ac024b84cb 76 currentState = HOMING;
jordiluong 0:80ac024b84cb 77 stateChanged = true;
jordiluong 0:80ac024b84cb 78 break;
jordiluong 0:80ac024b84cb 79 }
jordiluong 0:80ac024b84cb 80 }
jordiluong 0:80ac024b84cb 81
jordiluong 0:80ac024b84cb 82 case HOMING:
jordiluong 0:80ac024b84cb 83 {
jordiluong 0:80ac024b84cb 84 // State initialization
jordiluong 0:80ac024b84cb 85 if(stateChanged)
jordiluong 0:80ac024b84cb 86 {
jordiluong 0:80ac024b84cb 87 pc.printf("Entering HOMING \r\n");
jordiluong 0:80ac024b84cb 88 MoveToHome(); // Move to home position
jordiluong 0:80ac024b84cb 89 stateChanged = false;
jordiluong 0:80ac024b84cb 90 currentState = MOVING;
jordiluong 0:80ac024b84cb 91 stateChanged = true;
jordiluong 0:80ac024b84cb 92 break;
jordiluong 0:80ac024b84cb 93 }
jordiluong 0:80ac024b84cb 94 }
jordiluong 0:80ac024b84cb 95
jordiluong 0:80ac024b84cb 96 case MOVING:
jordiluong 0:80ac024b84cb 97 {
jordiluong 0:80ac024b84cb 98 // State initialization
jordiluong 0:80ac024b84cb 99 if(stateChanged)
jordiluong 0:80ac024b84cb 100 {
jordiluong 0:80ac024b84cb 101 pc.printf("Entering MOVING \r\n");
jordiluong 0:80ac024b84cb 102 stateChanged = false;
jordiluong 0:80ac024b84cb 103 }
jordiluong 0:80ac024b84cb 104
jordiluong 0:80ac024b84cb 105 // EMG signals to rotate motor 1
jordiluong 2:d3687b2c4e37 106 if(// EMG signal)
jordiluong 0:80ac024b84cb 107 {
jordiluong 2:d3687b2c4e37 108 FilterSignal(// Signal); // Filter the signal
jordiluong 2:d3687b2c4e37 109 RotateMotor1(// Voltage); // Rotate motor 1
jordiluong 0:80ac024b84cb 110 }
jordiluong 0:80ac024b84cb 111
jordiluong 0:80ac024b84cb 112 // EMG signals to rotate motor 2
jordiluong 2:d3687b2c4e37 113 if(// EMG signal)
jordiluong 0:80ac024b84cb 114 {
jordiluong 2:d3687b2c4e37 115 FilterSignal(// Signal); // Filter the signal
jordiluong 2:d3687b2c4e37 116 RotateMotor2(// Voltage); // Rotate motor 2
jordiluong 0:80ac024b84cb 117 }
jordiluong 0:80ac024b84cb 118
jordiluong 0:80ac024b84cb 119 // Hit command
jordiluong 2:d3687b2c4e37 120 if(// HIT COMMAND)
jordiluong 0:80ac024b84cb 121 {
jordiluong 0:80ac024b84cb 122 currentState = HITTING;
jordiluong 0:80ac024b84cb 123 stateChanged = true;
jordiluong 0:80ac024b84cb 124 break;
jordiluong 0:80ac024b84cb 125 }
jordiluong 0:80ac024b84cb 126 }
jordiluong 0:80ac024b84cb 127
jordiluong 0:80ac024b84cb 128 case HITTING:
jordiluong 0:80ac024b84cb 129 {
jordiluong 0:80ac024b84cb 130 // State initialization
jordiluong 0:80ac024b84cb 131 if(stateChanged)
jordiluong 0:80ac024b84cb 132 {
jordiluong 0:80ac024b84cb 133 pc.printf("Entering HITTING \r\n");
jordiluong 0:80ac024b84cb 134 HitBall(); // Hit the ball
jordiluong 0:80ac024b84cb 135 stateChanged = false;
jordiluong 0:80ac024b84cb 136 currentState = MOVING;
jordiluong 0:80ac024b84cb 137 stateChanged = true;
jordiluong 0:80ac024b84cb 138 break;
jordiluong 0:80ac024b84cb 139 }
jordiluong 0:80ac024b84cb 140 }
jordiluong 0:80ac024b84cb 141
jordiluong 0:80ac024b84cb 142 default:
jordiluong 0:80ac024b84cb 143 {
jordiluong 0:80ac024b84cb 144 TurnMotorsOff(); // Turn motors off for safety
jordiluong 0:80ac024b84cb 145 }
jordiluong 0:80ac024b84cb 146 }
jordiluong 0:80ac024b84cb 147 }
jordiluong 0:80ac024b84cb 148
jordiluong 0:80ac024b84cb 149 // Main function
jordiluong 0:80ac024b84cb 150 int main()
jordiluong 0:80ac024b84cb 151 {
jordiluong 0:80ac024b84cb 152 // Serial communication
jordiluong 0:80ac024b84cb 153 pc.baud(115200);
jordiluong 0:80ac024b84cb 154
jordiluong 0:80ac024b84cb 155 while(true)
jordiluong 0:80ac024b84cb 156 {
jordiluong 0:80ac024b84cb 157 ProcessStateMachine(); // Execute states function
jordiluong 0:80ac024b84cb 158 }
jordiluong 0:80ac024b84cb 159 }