Georgios Ardamerinos
/
DynamicsProject
Phlebot's onboard code
Fork of HelloWorld by
main.cpp@6:e69f8c6faebd, 2017-04-02 (annotated)
- Committer:
- gardamerinos
- Date:
- Sun Apr 02 06:06:39 2017 +0000
- Revision:
- 6:e69f8c6faebd
- Parent:
- 5:9f6830d1db7b
- Child:
- 7:b5053ba85843
Serial Attempt
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
simon | 0:fb6bbc10ffa0 | 1 | #include "mbed.h" |
gardamerinos | 6:e69f8c6faebd | 2 | //#include "BufferedSerial.h" |
gardamerinos | 4:1070977b83e9 | 3 | #include <vector> |
gardamerinos | 5:9f6830d1db7b | 4 | #include <string> |
gardamerinos | 5:9f6830d1db7b | 5 | #include <sstream> |
gardamerinos | 5:9f6830d1db7b | 6 | #include <iostream> |
simon | 0:fb6bbc10ffa0 | 7 | |
gardamerinos | 5:9f6830d1db7b | 8 | //====================ALL DATA======================================== |
gardamerinos | 4:1070977b83e9 | 9 | vector<DigitalOut*> motors; |
gardamerinos | 5:9f6830d1db7b | 10 | vector<DigitalOut*> dirs; |
gardamerinos | 6:e69f8c6faebd | 11 | vector<DigitalIn*> switches; |
gardamerinos | 4:1070977b83e9 | 12 | vector<int> steps; |
gardamerinos | 5:9f6830d1db7b | 13 | vector<int> currentPosition; |
gardamerinos | 5:9f6830d1db7b | 14 | vector<int> goalPosition; |
gardamerinos | 6:e69f8c6faebd | 15 | vector<int> buffer; |
gardamerinos | 6:e69f8c6faebd | 16 | bool negativeDirs [4] = {1,0,0,0}; |
gardamerinos | 4:1070977b83e9 | 17 | |
gardamerinos | 5:9f6830d1db7b | 18 | //====================SERIAL COMMUNICATION============================= |
gardamerinos | 5:9f6830d1db7b | 19 | Serial pc(USBTX, USBRX); |
gardamerinos | 5:9f6830d1db7b | 20 | |
gardamerinos | 5:9f6830d1db7b | 21 | //===================VARIABLES======================================== |
gardamerinos | 6:e69f8c6faebd | 22 | int delayus = 200; |
gardamerinos | 5:9f6830d1db7b | 23 | int delayusOffset = delayus - 20; |
gardamerinos | 6:e69f8c6faebd | 24 | int stepsX = 0; |
gardamerinos | 6:e69f8c6faebd | 25 | int stepsY = 0; |
gardamerinos | 6:e69f8c6faebd | 26 | int stepsZ = 0; |
gardamerinos | 6:e69f8c6faebd | 27 | int stepsRot = 0; |
gardamerinos | 6:e69f8c6faebd | 28 | int mmToSteps = 1; |
gardamerinos | 6:e69f8c6faebd | 29 | int degToSteps = 1; |
gardamerinos | 6:e69f8c6faebd | 30 | DigitalOut led(LED1); |
gardamerinos | 5:9f6830d1db7b | 31 | //======================FUNCTIONS======================================= |
gardamerinos | 6:e69f8c6faebd | 32 | void step(DigitalOut stepsPin,int voltage) |
gardamerinos | 6:e69f8c6faebd | 33 | { |
gardamerinos | 4:1070977b83e9 | 34 | stepsPin = voltage; |
gardamerinos | 4:1070977b83e9 | 35 | } |
simon | 0:fb6bbc10ffa0 | 36 | |
gardamerinos | 6:e69f8c6faebd | 37 | void setDirections(vector<DigitalOut*> dirPin, bool voltage[4]) |
gardamerinos | 6:e69f8c6faebd | 38 | { |
gardamerinos | 6:e69f8c6faebd | 39 | for (int i = 0; i < 4; i++) { |
gardamerinos | 5:9f6830d1db7b | 40 | *dirPin[i] = voltage[i]; |
gardamerinos | 5:9f6830d1db7b | 41 | } |
gardamerinos | 5:9f6830d1db7b | 42 | } |
gardamerinos | 5:9f6830d1db7b | 43 | |
gardamerinos | 6:e69f8c6faebd | 44 | void moveOneStep() |
gardamerinos | 6:e69f8c6faebd | 45 | { |
gardamerinos | 6:e69f8c6faebd | 46 | for (int i = 0; i<motors.size(); i++) { |
gardamerinos | 6:e69f8c6faebd | 47 | if (steps.at(i) <= int(0)) { |
gardamerinos | 6:e69f8c6faebd | 48 | motors.erase(motors.begin() + i); |
gardamerinos | 6:e69f8c6faebd | 49 | steps.erase(steps.begin()+i); |
gardamerinos | 6:e69f8c6faebd | 50 | switches.erase(switches.begin()+i); |
gardamerinos | 4:1070977b83e9 | 51 | } |
gardamerinos | 6:e69f8c6faebd | 52 | } |
gardamerinos | 6:e69f8c6faebd | 53 | for (int i = 0; i<motors.size(); i++) { |
gardamerinos | 6:e69f8c6faebd | 54 | step(*motors[i],1); |
gardamerinos | 6:e69f8c6faebd | 55 | } |
gardamerinos | 6:e69f8c6faebd | 56 | wait_us(delayus); |
gardamerinos | 6:e69f8c6faebd | 57 | for (int i = 0; i<motors.size(); i++) { |
gardamerinos | 6:e69f8c6faebd | 58 | step(*motors.at(i),0); |
gardamerinos | 6:e69f8c6faebd | 59 | steps.at(i) = steps.at(i)-1; |
gardamerinos | 6:e69f8c6faebd | 60 | } |
gardamerinos | 6:e69f8c6faebd | 61 | wait_us(delayusOffset); |
gardamerinos | 5:9f6830d1db7b | 62 | } |
gardamerinos | 5:9f6830d1db7b | 63 | |
gardamerinos | 6:e69f8c6faebd | 64 | void goHome() |
gardamerinos | 6:e69f8c6faebd | 65 | { |
gardamerinos | 6:e69f8c6faebd | 66 | while(!(*switches.at(0)) || !(*switches.at(1)) || !(*switches.at(2)) || !(*switches.at(3))) { |
gardamerinos | 6:e69f8c6faebd | 67 | for (int i = 0; i < motors.size(); i++) { |
gardamerinos | 6:e69f8c6faebd | 68 | if (!(*switches.at(i))) { |
gardamerinos | 6:e69f8c6faebd | 69 | steps.at(i)= steps.at(i)+1; |
gardamerinos | 6:e69f8c6faebd | 70 | } |
gardamerinos | 6:e69f8c6faebd | 71 | } |
gardamerinos | 6:e69f8c6faebd | 72 | moveOneStep(); |
gardamerinos | 5:9f6830d1db7b | 73 | } |
gardamerinos | 5:9f6830d1db7b | 74 | } |
gardamerinos | 6:e69f8c6faebd | 75 | void goHomeRotation() |
gardamerinos | 6:e69f8c6faebd | 76 | { |
gardamerinos | 6:e69f8c6faebd | 77 | while(!(*switches.at(3))) { |
gardamerinos | 6:e69f8c6faebd | 78 | step(*motors[3],1); |
gardamerinos | 6:e69f8c6faebd | 79 | wait_us(8000); |
gardamerinos | 6:e69f8c6faebd | 80 | step(*motors.at(3),0); |
gardamerinos | 6:e69f8c6faebd | 81 | wait_us(8000); |
simon | 0:fb6bbc10ffa0 | 82 | } |
simon | 0:fb6bbc10ffa0 | 83 | } |
gardamerinos | 5:9f6830d1db7b | 84 | |
gardamerinos | 6:e69f8c6faebd | 85 | int millimetersToSteps(int mm){ |
gardamerinos | 6:e69f8c6faebd | 86 | int steps = mmToSteps*mm; |
gardamerinos | 6:e69f8c6faebd | 87 | return steps; |
gardamerinos | 6:e69f8c6faebd | 88 | } |
gardamerinos | 6:e69f8c6faebd | 89 | |
gardamerinos | 6:e69f8c6faebd | 90 | int degreesToSteps(int degrees){ |
gardamerinos | 6:e69f8c6faebd | 91 | int steps = degToSteps*degrees; |
gardamerinos | 6:e69f8c6faebd | 92 | return steps; |
gardamerinos | 6:e69f8c6faebd | 93 | } |
gardamerinos | 6:e69f8c6faebd | 94 | |
gardamerinos | 6:e69f8c6faebd | 95 | void new_directions(){ |
gardamerinos | 6:e69f8c6faebd | 96 | bool newDirs [4]; |
gardamerinos | 6:e69f8c6faebd | 97 | for (int i=0; i<goalPosition.size();i++){ |
gardamerinos | 6:e69f8c6faebd | 98 | if (goalPosition.at(i) >=0){ |
gardamerinos | 6:e69f8c6faebd | 99 | newDirs[i] = !negativeDirs[i]; |
gardamerinos | 6:e69f8c6faebd | 100 | } |
gardamerinos | 6:e69f8c6faebd | 101 | else{ |
gardamerinos | 6:e69f8c6faebd | 102 | newDirs[i] = negativeDirs[i]; |
gardamerinos | 6:e69f8c6faebd | 103 | } |
gardamerinos | 6:e69f8c6faebd | 104 | } |
gardamerinos | 6:e69f8c6faebd | 105 | setDirections(dirs, newDirs); |
gardamerinos | 6:e69f8c6faebd | 106 | } |
gardamerinos | 6:e69f8c6faebd | 107 | |
gardamerinos | 6:e69f8c6faebd | 108 | void initialization() |
gardamerinos | 6:e69f8c6faebd | 109 | { |
gardamerinos | 5:9f6830d1db7b | 110 | motors.push_back(new DigitalOut(p21)); |
gardamerinos | 5:9f6830d1db7b | 111 | motors.push_back(new DigitalOut(p22)); |
gardamerinos | 5:9f6830d1db7b | 112 | motors.push_back(new DigitalOut(p23)); |
gardamerinos | 5:9f6830d1db7b | 113 | motors.push_back(new DigitalOut(p24)); |
gardamerinos | 5:9f6830d1db7b | 114 | steps.push_back(stepsX); |
gardamerinos | 5:9f6830d1db7b | 115 | steps.push_back(stepsY); |
gardamerinos | 5:9f6830d1db7b | 116 | steps.push_back(stepsZ); |
gardamerinos | 5:9f6830d1db7b | 117 | steps.push_back(stepsRot); |
gardamerinos | 5:9f6830d1db7b | 118 | dirs.push_back(new DigitalOut(p30)); |
gardamerinos | 5:9f6830d1db7b | 119 | dirs.push_back(new DigitalOut(p29)); |
gardamerinos | 5:9f6830d1db7b | 120 | dirs.push_back(new DigitalOut(p28)); |
gardamerinos | 5:9f6830d1db7b | 121 | dirs.push_back(new DigitalOut(p27)); |
gardamerinos | 6:e69f8c6faebd | 122 | switches.push_back(new DigitalIn(p15)); |
gardamerinos | 6:e69f8c6faebd | 123 | switches.push_back(new DigitalIn(p16)); |
gardamerinos | 6:e69f8c6faebd | 124 | switches.push_back(new DigitalIn(p17)); |
gardamerinos | 6:e69f8c6faebd | 125 | switches.push_back(new DigitalIn(p18)); |
gardamerinos | 6:e69f8c6faebd | 126 | switches[0]->mode(PullUp); |
gardamerinos | 6:e69f8c6faebd | 127 | switches[1]->mode(PullUp); |
gardamerinos | 6:e69f8c6faebd | 128 | switches[2]->mode(PullUp); |
gardamerinos | 6:e69f8c6faebd | 129 | switches[3]->mode(PullUp); |
gardamerinos | 6:e69f8c6faebd | 130 | } |
gardamerinos | 6:e69f8c6faebd | 131 | |
gardamerinos | 6:e69f8c6faebd | 132 | void re_initialization(){ |
gardamerinos | 6:e69f8c6faebd | 133 | motors.push_back(new DigitalOut(p21)); |
gardamerinos | 6:e69f8c6faebd | 134 | motors.push_back(new DigitalOut(p22)); |
gardamerinos | 6:e69f8c6faebd | 135 | motors.push_back(new DigitalOut(p23)); |
gardamerinos | 6:e69f8c6faebd | 136 | motors.push_back(new DigitalOut(p24)); |
gardamerinos | 6:e69f8c6faebd | 137 | steps.push_back(millimetersToSteps(goalPosition.at(0))); |
gardamerinos | 6:e69f8c6faebd | 138 | steps.push_back(millimetersToSteps(goalPosition.at(1))); |
gardamerinos | 6:e69f8c6faebd | 139 | steps.push_back(millimetersToSteps(goalPosition.at(2))); |
gardamerinos | 6:e69f8c6faebd | 140 | steps.push_back(degreesToSteps(goalPosition.at(3))); |
gardamerinos | 6:e69f8c6faebd | 141 | switches.push_back(new DigitalIn(p15)); |
gardamerinos | 6:e69f8c6faebd | 142 | switches.push_back(new DigitalIn(p16)); |
gardamerinos | 6:e69f8c6faebd | 143 | switches.push_back(new DigitalIn(p17)); |
gardamerinos | 6:e69f8c6faebd | 144 | switches.push_back(new DigitalIn(p18)); |
gardamerinos | 6:e69f8c6faebd | 145 | switches[0]->mode(PullUp); |
gardamerinos | 6:e69f8c6faebd | 146 | switches[1]->mode(PullUp); |
gardamerinos | 6:e69f8c6faebd | 147 | switches[2]->mode(PullUp); |
gardamerinos | 6:e69f8c6faebd | 148 | switches[3]->mode(PullUp); |
gardamerinos | 6:e69f8c6faebd | 149 | } |
gardamerinos | 6:e69f8c6faebd | 150 | |
gardamerinos | 6:e69f8c6faebd | 151 | void serial_check() |
gardamerinos | 6:e69f8c6faebd | 152 | { |
gardamerinos | 6:e69f8c6faebd | 153 | if (pc.readable()){ |
gardamerinos | 6:e69f8c6faebd | 154 | int numberIn; |
gardamerinos | 6:e69f8c6faebd | 155 | |
gardamerinos | 6:e69f8c6faebd | 156 | char *dataStart = (char *)&numberIn; |
gardamerinos | 6:e69f8c6faebd | 157 | char byteIn; |
gardamerinos | 6:e69f8c6faebd | 158 | |
gardamerinos | 6:e69f8c6faebd | 159 | for (int i = 0; i < 2; i++) { |
gardamerinos | 6:e69f8c6faebd | 160 | byteIn = pc.getc(); |
gardamerinos | 6:e69f8c6faebd | 161 | *(dataStart + i) = byteIn; |
gardamerinos | 6:e69f8c6faebd | 162 | } |
gardamerinos | 6:e69f8c6faebd | 163 | buffer.push_back(numberIn); |
gardamerinos | 6:e69f8c6faebd | 164 | if (buffer.size()== 5){ |
gardamerinos | 6:e69f8c6faebd | 165 | //state machine at some point |
gardamerinos | 6:e69f8c6faebd | 166 | goalPosition.clear(); |
gardamerinos | 6:e69f8c6faebd | 167 | for (int i = 1; i<5; i++){ |
gardamerinos | 6:e69f8c6faebd | 168 | goalPosition.push_back(buffer.at(i)); |
gardamerinos | 6:e69f8c6faebd | 169 | } |
gardamerinos | 6:e69f8c6faebd | 170 | re_initialization(); |
gardamerinos | 6:e69f8c6faebd | 171 | buffer.clear(); |
gardamerinos | 6:e69f8c6faebd | 172 | } |
gardamerinos | 6:e69f8c6faebd | 173 | } |
gardamerinos | 5:9f6830d1db7b | 174 | } |
gardamerinos | 5:9f6830d1db7b | 175 | |
gardamerinos | 5:9f6830d1db7b | 176 | //=================MAIN======================================== |
gardamerinos | 6:e69f8c6faebd | 177 | int main() |
gardamerinos | 6:e69f8c6faebd | 178 | { |
gardamerinos | 5:9f6830d1db7b | 179 | initialization(); |
gardamerinos | 6:e69f8c6faebd | 180 | // DigitalOut led(LED1); |
gardamerinos | 6:e69f8c6faebd | 181 | setDirections(dirs, negativeDirs); |
gardamerinos | 6:e69f8c6faebd | 182 | goHomeRotation(); |
gardamerinos | 6:e69f8c6faebd | 183 | goHome(); |
gardamerinos | 5:9f6830d1db7b | 184 | while(1) { |
gardamerinos | 6:e69f8c6faebd | 185 | serial_check(); |
gardamerinos | 5:9f6830d1db7b | 186 | } |
gardamerinos | 5:9f6830d1db7b | 187 | } |