Georgios Ardamerinos
/
DynamicsProject
Phlebot's onboard code
Fork of HelloWorld by
main.cpp
- Committer:
- gardamerinos
- Date:
- 2017-05-01
- Revision:
- 8:dc3d3dfc777e
- Parent:
- 7:b5053ba85843
File content as of revision 8:dc3d3dfc777e:
#include "mbed.h" #include <vector> #include <string> #include <sstream> #include <iostream> //====================ALL DATA======================================== vector<DigitalOut*> motors; vector<DigitalOut*> dirs; vector<DigitalIn*> switches; vector<int> steps; vector<int> currentPosition; vector<int> goalPosition; vector<int> buffer; bool negativeDirs [4] = {1,1,0,0}; bool positiveDirs [4] = {0,0,1,1}; //====================SERIAL COMMUNICATION============================= Serial pc(USBTX, USBRX); //===================VARIABLES======================================== int delayus = 200; int delayusOffset = delayus - 20; int stepsX = 0; int stepsY = 0; int stepsZ = 0; int stepsRot = 0; int mmToSteps = 2400; int degToSteps = 4.444; DigitalOut led(LED1); //======================FUNCTIONS======================================= void step(DigitalOut stepsPin,int voltage) { stepsPin = voltage; } void setDirections(vector<DigitalOut*> dirPin, bool voltage[4]) { for (int i = 0; i < 4; i++) { *dirPin[i] = voltage[i]; } } void moveOneStep() { for (int i = 0; i<motors.size(); i++) { if (steps.at(i) <= int(0)) { motors.erase(motors.begin() + i); steps.erase(steps.begin()+i); switches.erase(switches.begin()+i); } } for (int i = 0; i<motors.size(); i++) { step(*motors[i],1); } wait_us(delayus); for (int i = 0; i<motors.size(); i++) { step(*motors.at(i),0); steps.at(i) = steps.at(i)-1; } wait_us(delayusOffset); } void goHome() { while(!(*switches.at(0)) || !(*switches.at(1)) || !(*switches.at(2)) || !(*switches.at(3))) { for (int i = 0; i < motors.size(); i++) { if (!(*switches.at(i))) { steps.at(i)= steps.at(i)+1; } } moveOneStep(); } } void goHomeRotation() { while(!(*switches.at(3))) { step(*motors[3],1); wait_us(8000); step(*motors.at(3),0); wait_us(8000); } } int millimetersToSteps(int mm){ int steps = mmToSteps*mm; return steps; } int degreesToSteps(int degrees){ int steps = degToSteps*degrees; return steps; } void new_directions(){ bool newDirs [4]; for (int i=0; i<goalPosition.size();i++){ if (goalPosition.at(i) >=0){ newDirs[i] = !negativeDirs[i]; } else{ newDirs[i] = negativeDirs[i]; } } setDirections(dirs, newDirs); } void initialization() { motors.push_back(new DigitalOut(p21)); motors.push_back(new DigitalOut(p22)); motors.push_back(new DigitalOut(p23)); motors.push_back(new DigitalOut(p24)); steps.push_back(stepsX); steps.push_back(stepsY); steps.push_back(stepsZ); steps.push_back(stepsRot); dirs.push_back(new DigitalOut(p30)); dirs.push_back(new DigitalOut(p29)); dirs.push_back(new DigitalOut(p28)); dirs.push_back(new DigitalOut(p27)); switches.push_back(new DigitalIn(p15)); switches.push_back(new DigitalIn(p16)); switches.push_back(new DigitalIn(p17)); switches.push_back(new DigitalIn(p18)); switches[0]->mode(PullUp); switches[1]->mode(PullUp); switches[2]->mode(PullUp); switches[3]->mode(PullUp); } void re_initialization(){ motors.clear(); motors.push_back(new DigitalOut(p21)); motors.push_back(new DigitalOut(p22)); motors.push_back(new DigitalOut(p23)); motors.push_back(new DigitalOut(p24)); steps.clear(); steps.push_back(millimetersToSteps(goalPosition.at(0))); steps.push_back(millimetersToSteps(goalPosition.at(1))); steps.push_back(millimetersToSteps(goalPosition.at(2))); steps.push_back(degreesToSteps(goalPosition.at(3))); switches.clear(); switches.push_back(new DigitalIn(p15)); switches.push_back(new DigitalIn(p16)); switches.push_back(new DigitalIn(p17)); switches.push_back(new DigitalIn(p18)); switches[0]->mode(PullUp); switches[1]->mode(PullUp); switches[2]->mode(PullUp); switches[3]->mode(PullUp); } void serial_check() { if (pc.readable()){ int numberIn; // pc.putc(pc.getc()); char *dataStart = (char *)&numberIn; char byteIn; for (int i = 0; i < 2; i++) { byteIn = pc.getc(); pc.putc(byteIn); *(dataStart + i) = byteIn; } buffer.push_back(numberIn); if (buffer.size()== 5){ pc.putc(byteIn); //state machine at some point goalPosition.clear(); for (int i = 1; i<5; i++){ goalPosition.push_back(buffer.at(i)); } re_initialization(); buffer.clear(); setDirections(dirs, positiveDirs); while(motors.size()>0){ moveOneStep(); wait_us(1000); } } } } //=================MAIN======================================== int main() { pc.baud(19200); initialization(); DigitalOut led(LED1); setDirections(dirs, negativeDirs); goHomeRotation(); goHome(); while(1) { serial_check(); // led = *switches[1]; // moveOneStep(); } }