Diff: main.cpp

Revision:

6:e69f8c6faebd

Parent:

5:9f6830d1db7b

Child:

7:b5053ba85843

--- a/main.cpp	Fri Feb 24 06:18:49 2017 +0000
+++ b/main.cpp	Sun Apr 02 06:06:39 2017 +0000
@@ -1,4 +1,5 @@
 #include "mbed.h"
+//#include "BufferedSerial.h"
 #include <vector>
 #include <string>
 #include <sstream>
@@ -7,73 +8,105 @@
 //====================ALL DATA========================================
 vector<DigitalOut*> motors;
 vector<DigitalOut*> dirs;
+vector<DigitalIn*> switches;
 vector<int> steps;
 vector<int> currentPosition;
 vector<int> goalPosition;
-int positiveDirs [4] = {1,1,1,1};
+vector<int> buffer;
+bool negativeDirs [4] = {1,0,0,0};
 
 //====================SERIAL COMMUNICATION=============================
 Serial pc(USBTX, USBRX);
 
 //===================VARIABLES========================================
-int delayus = 100;
+int delayus = 200;
 int delayusOffset = delayus - 20;
-int stepsX = 5000;
-int stepsY = 5000;
-int stepsZ = 5000;
-int stepsRot = 100;
-string msg;
-
+int stepsX = 0;
+int stepsY = 0;
+int stepsZ = 0;
+int stepsRot = 0;
+int mmToSteps = 1;
+int degToSteps = 1;
+DigitalOut led(LED1);
 //======================FUNCTIONS=======================================
-void step(DigitalOut stepsPin,int voltage){
+void step(DigitalOut stepsPin,int voltage)
+{
     stepsPin = voltage;
 }
 
-void setDirections(vector<DigitalOut*> dirPin, int voltage[4]){
-    for (int i = 0; i < 4; i++){
+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);
-            }
+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);
+    }
+    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);
 }
 
-vector<string> splitString(string input){
-    istringstream iss(input);
-    vector<string> output;
-    while (iss) {
-        string word;
-        iss >> word;
-        output.push_back(word);
+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();
     }
-    return output;
 }
-
-void callback(){
-    pc.scanf("%s", &msg);
-    vector<string> stringPos= splitString(msg);
-    goalPosition.clear();
-    for (int i = 0; i<stringPos.size(); i++){
-        goalPosition.push_back(atoi(stringPos[i].c_str()));
+void goHomeRotation()
+{
+    while(!(*switches.at(3))) {
+        step(*motors[3],1);
+        wait_us(8000);
+        step(*motors.at(3),0);
+        wait_us(8000);
     }
 }
 
-void initialization(){
+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));
@@ -86,16 +119,69 @@
     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.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(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.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;
+     
+        char *dataStart = (char *)&numberIn;
+        char byteIn;
+     
+        for (int i = 0; i < 2; i++) {
+            byteIn = pc.getc();
+            *(dataStart + i) = byteIn;
+        }
+        buffer.push_back(numberIn);
+        if (buffer.size()== 5){
+            //state machine at some point
+            goalPosition.clear();
+            for (int i = 1; i<5; i++){
+                goalPosition.push_back(buffer.at(i));
+            }
+            re_initialization();
+            buffer.clear();
+        }
+    }
 }
 
 //=================MAIN========================================
-int main() {
+int main()
+{
     initialization();
-    setDirections(dirs, positiveDirs);
-    pc.attach(&callback);
-    
+//    DigitalOut led(LED1);
+    setDirections(dirs, negativeDirs);
+    goHomeRotation();
+    goHome();
     while(1) {
-        moveOneStep();
-        
+        serial_check();
     }
 }