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();
    }
}