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@3:5c3edcd29448, 2017-10-09 (annotated)

Committer:
jordiluong
Date:
Mon Oct 09 13:59:45 2017 +0000
Revision:
3:5c3edcd29448
Parent:
2:d3687b2c4e37
Child:
4:ea7689bf97e1
Added functions for motors (TEST VERSION)

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 3:5c3edcd29448 17 // PINS
jordiluong 3:5c3edcd29448 18 DigitalOut motor1DirectionPin(D4); // Value 0: CW or CCW?; 1: CW or CCW?
jordiluong 3:5c3edcd29448 19 PwmOut motor1MagnitudePin(D5);
jordiluong 3:5c3edcd29448 20 DigitalOut motor2DirectionPin(D7); // Value 0: CW or CCW?; 1: CW or CCW?
jordiluong 3:5c3edcd29448 21 PwmOut motor2MagnitudePin(D6);
jordiluong 3:5c3edcd29448 22 InterruptIn button1(D2); // BUTTON 1 AANSLUITEN OP D2!!!
jordiluong 3:5c3edcd29448 23 InterruptIn button2(D3); // BUTTON 2 AANSLUITEN OP D3!!!
jordiluong 3:5c3edcd29448 24
jordiluong 0:80ac024b84cb 25 // DEFINITIONS
jordiluong 3:5c3edcd29448 26 const float motorVelocity = 4; // rad/s
jordiluong 3:5c3edcd29448 27 const float motorGain = 4; // (rad/s) / PWM
jordiluong 0:80ac024b84cb 28
jordiluong 0:80ac024b84cb 29 // FUNCTIONS
jordiluong 0:80ac024b84cb 30 // Turn motors off
jordiluong 3:5c3edcd29448 31 /*void TurnMotorsOff()
jordiluong 0:80ac024b84cb 32 {
jordiluong 2:d3687b2c4e37 33 // Turn motors off
jordiluong 0:80ac024b84cb 34 }
jordiluong 0:80ac024b84cb 35
jordiluong 0:80ac024b84cb 36 // Move to home
jordiluong 0:80ac024b84cb 37 void MoveToHome()
jordiluong 0:80ac024b84cb 38 {
jordiluong 2:d3687b2c4e37 39 // Move to home
jordiluong 0:80ac024b84cb 40 }
jordiluong 0:80ac024b84cb 41
jordiluong 0:80ac024b84cb 42 // Filter signals
jordiluong 2:d3687b2c4e37 43 float FilterSignal(// Signal)
jordiluong 0:80ac024b84cb 44 {
jordiluong 2:d3687b2c4e37 45 // Filter signal
jordiluong 2:d3687b2c4e37 46 return // Voltage
jordiluong 0:80ac024b84cb 47 }
jordiluong 0:80ac024b84cb 48
jordiluong 0:80ac024b84cb 49 // Motor 1
jordiluong 2:d3687b2c4e37 50 void RotateMotor1(// Voltage)
jordiluong 0:80ac024b84cb 51 {
jordiluong 2:d3687b2c4e37 52 // Rotate motor 1
jordiluong 0:80ac024b84cb 53 }
jordiluong 0:80ac024b84cb 54
jordiluong 0:80ac024b84cb 55 // Motor 2
jordiluong 2:d3687b2c4e37 56 void RotateMotor2(// Voltage)
jordiluong 0:80ac024b84cb 57 {
jordiluong 2:d3687b2c4e37 58 // Rotate motor 2
jordiluong 0:80ac024b84cb 59 }
jordiluong 0:80ac024b84cb 60
jordiluong 0:80ac024b84cb 61 // Hit the ball
jordiluong 0:80ac024b84cb 62 void HitBall()
jordiluong 0:80ac024b84cb 63 {
jordiluong 2:d3687b2c4e37 64 // Rotate motor 3
jordiluong 0:80ac024b84cb 65 }
jordiluong 3:5c3edcd29448 66 */
jordiluong 3:5c3edcd29448 67
jordiluong 3:5c3edcd29448 68 void TurnMotor1CW()
jordiluong 3:5c3edcd29448 69 {
jordiluong 3:5c3edcd29448 70 motor1DirectionPin = 0;
jordiluong 3:5c3edcd29448 71 motor1MagnitudePin = fabs(motorVelocity / motorGain);
jordiluong 3:5c3edcd29448 72 }
jordiluong 3:5c3edcd29448 73
jordiluong 3:5c3edcd29448 74 void TurnMotor1CCW()
jordiluong 3:5c3edcd29448 75 {
jordiluong 3:5c3edcd29448 76 motor1DirectionPin = 1;
jordiluong 3:5c3edcd29448 77 motor1MagnitudePin = fabs(motorVelocity / motorGain);
jordiluong 3:5c3edcd29448 78 }
jordiluong 3:5c3edcd29448 79
jordiluong 3:5c3edcd29448 80 void TurnMotorsOff()
jordiluong 3:5c3edcd29448 81 {
jordiluong 3:5c3edcd29448 82 motor1MagnitudePin = 0;
jordiluong 3:5c3edcd29448 83 }
jordiluong 0:80ac024b84cb 84
jordiluong 0:80ac024b84cb 85 // States function
jordiluong 0:80ac024b84cb 86 void ProcessStateMachine()
jordiluong 0:80ac024b84cb 87 {
jordiluong 0:80ac024b84cb 88 switch(currentState)
jordiluong 0:80ac024b84cb 89 {
jordiluong 0:80ac024b84cb 90 case MOTORS_OFF:
jordiluong 0:80ac024b84cb 91 {
jordiluong 0:80ac024b84cb 92 // State initialization
jordiluong 0:80ac024b84cb 93 if(stateChanged)
jordiluong 0:80ac024b84cb 94 {
jordiluong 0:80ac024b84cb 95 pc.printf("Entering MOTORS_OFF \r\n");
jordiluong 0:80ac024b84cb 96 TurnMotorsOff(); // Turn motors off
jordiluong 0:80ac024b84cb 97 stateChanged = false;
jordiluong 0:80ac024b84cb 98 }
jordiluong 0:80ac024b84cb 99
jordiluong 0:80ac024b84cb 100 // Home command
jordiluong 3:5c3edcd29448 101 if(button1)
jordiluong 0:80ac024b84cb 102 {
jordiluong 0:80ac024b84cb 103 currentState = MOVING;
jordiluong 0:80ac024b84cb 104 stateChanged = true;
jordiluong 0:80ac024b84cb 105 break;
jordiluong 0:80ac024b84cb 106 }
jordiluong 0:80ac024b84cb 107 }
jordiluong 0:80ac024b84cb 108
jordiluong 0:80ac024b84cb 109 case MOVING:
jordiluong 0:80ac024b84cb 110 {
jordiluong 0:80ac024b84cb 111 // State initialization
jordiluong 0:80ac024b84cb 112 if(stateChanged)
jordiluong 0:80ac024b84cb 113 {
jordiluong 0:80ac024b84cb 114 pc.printf("Entering MOVING \r\n");
jordiluong 0:80ac024b84cb 115 stateChanged = false;
jordiluong 0:80ac024b84cb 116 }
jordiluong 3:5c3edcd29448 117 /*
jordiluong 0:80ac024b84cb 118 // EMG signals to rotate motor 1
jordiluong 2:d3687b2c4e37 119 if(// EMG signal)
jordiluong 0:80ac024b84cb 120 {
jordiluong 2:d3687b2c4e37 121 FilterSignal(// Signal); // Filter the signal
jordiluong 2:d3687b2c4e37 122 RotateMotor1(// Voltage); // Rotate motor 1
jordiluong 0:80ac024b84cb 123 }
jordiluong 0:80ac024b84cb 124
jordiluong 0:80ac024b84cb 125 // EMG signals to rotate motor 2
jordiluong 2:d3687b2c4e37 126 if(// EMG signal)
jordiluong 0:80ac024b84cb 127 {
jordiluong 2:d3687b2c4e37 128 FilterSignal(// Signal); // Filter the signal
jordiluong 2:d3687b2c4e37 129 RotateMotor2(// Voltage); // Rotate motor 2
jordiluong 0:80ac024b84cb 130 }
jordiluong 0:80ac024b84cb 131
jordiluong 0:80ac024b84cb 132 // Hit command
jordiluong 2:d3687b2c4e37 133 if(// HIT COMMAND)
jordiluong 0:80ac024b84cb 134 {
jordiluong 0:80ac024b84cb 135 currentState = HITTING;
jordiluong 0:80ac024b84cb 136 stateChanged = true;
jordiluong 0:80ac024b84cb 137 break;
jordiluong 0:80ac024b84cb 138 }
jordiluong 3:5c3edcd29448 139 */
jordiluong 3:5c3edcd29448 140 // Motor testen
jordiluong 3:5c3edcd29448 141 while(button1)
jordiluong 3:5c3edcd29448 142 {
jordiluong 3:5c3edcd29448 143 TurnMotor1CW();
jordiluong 3:5c3edcd29448 144 pc.printf("Turning motor 1 CW \r\n");
jordiluong 3:5c3edcd29448 145 }
jordiluong 3:5c3edcd29448 146
jordiluong 3:5c3edcd29448 147 while(button2)
jordiluong 3:5c3edcd29448 148 {
jordiluong 3:5c3edcd29448 149 TurnMotor1CCW();
jordiluong 3:5c3edcd29448 150 pc.printf("Turning motor 1 CounterCW \r\n");
jordiluong 3:5c3edcd29448 151 }
jordiluong 3:5c3edcd29448 152
jordiluong 3:5c3edcd29448 153 TurnMotorsOff();
jordiluong 0:80ac024b84cb 154 }
jordiluong 0:80ac024b84cb 155
jordiluong 0:80ac024b84cb 156 case HITTING:
jordiluong 0:80ac024b84cb 157 {
jordiluong 0:80ac024b84cb 158 // State initialization
jordiluong 0:80ac024b84cb 159 if(stateChanged)
jordiluong 0:80ac024b84cb 160 {
jordiluong 0:80ac024b84cb 161 pc.printf("Entering HITTING \r\n");
jordiluong 3:5c3edcd29448 162 //HitBall(); // Hit the ball
jordiluong 0:80ac024b84cb 163 stateChanged = false;
jordiluong 0:80ac024b84cb 164 currentState = MOVING;
jordiluong 0:80ac024b84cb 165 stateChanged = true;
jordiluong 0:80ac024b84cb 166 break;
jordiluong 0:80ac024b84cb 167 }
jordiluong 0:80ac024b84cb 168 }
jordiluong 0:80ac024b84cb 169
jordiluong 0:80ac024b84cb 170 default:
jordiluong 0:80ac024b84cb 171 {
jordiluong 0:80ac024b84cb 172 TurnMotorsOff(); // Turn motors off for safety
jordiluong 0:80ac024b84cb 173 }
jordiluong 0:80ac024b84cb 174 }
jordiluong 0:80ac024b84cb 175 }
jordiluong 0:80ac024b84cb 176
jordiluong 0:80ac024b84cb 177 // Main function
jordiluong 0:80ac024b84cb 178 int main()
jordiluong 0:80ac024b84cb 179 {
jordiluong 0:80ac024b84cb 180 // Serial communication
jordiluong 0:80ac024b84cb 181 pc.baud(115200);
jordiluong 0:80ac024b84cb 182
jordiluong 0:80ac024b84cb 183 while(true)
jordiluong 0:80ac024b84cb 184 {
jordiluong 0:80ac024b84cb 185 ProcessStateMachine(); // Execute states function
jordiluong 0:80ac024b84cb 186 }
jordiluong 0:80ac024b84cb 187 }