Diff: main.cpp

Revision:

5:9f6830d1db7b

Parent:

4:1070977b83e9

Child:

6:e69f8c6faebd

--- a/main.cpp	Fri Feb 24 03:20:46 2017 +0000
+++ b/main.cpp	Fri Feb 24 06:18:49 2017 +0000
@@ -1,34 +1,42 @@
 #include "mbed.h"
 #include <vector>
+#include <string>
+#include <sstream>
+#include <iostream>
 
+//====================ALL DATA========================================
 vector<DigitalOut*> motors;
+vector<DigitalOut*> dirs;
 vector<int> steps;
+vector<int> currentPosition;
+vector<int> goalPosition;
+int positiveDirs [4] = {1,1,1,1};
 
+//====================SERIAL COMMUNICATION=============================
+Serial pc(USBTX, USBRX);
+
+//===================VARIABLES========================================
+int delayus = 100;
+int delayusOffset = delayus - 20;
+int stepsX = 5000;
+int stepsY = 5000;
+int stepsZ = 5000;
+int stepsRot = 100;
+string msg;
+
+//======================FUNCTIONS=======================================
 void step(DigitalOut stepsPin,int voltage){
     stepsPin = voltage;
 }
 
-int main() {
-    DigitalOut stepperX_DIR(p30);
-    DigitalOut stepperY_DIR(p29);
-    DigitalOut stepperZ_DIR(p28);
-    DigitalOut stepperRot_DIR(p27);
-    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(5000);
-    steps.push_back(5000);
-    steps.push_back(5000);
-    steps.push_back(100);
-    
-    stepperX_DIR = 1;
-    stepperY_DIR = 1;
-    stepperZ_DIR = 1;
-    stepperRot_DIR = 1;
-    
-    while(1) {
-        for (int i = 0; i<motors.size();i++){
+void setDirections(vector<DigitalOut*> dirPin, int 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);
@@ -37,10 +45,57 @@
         for (int i = 0; i<motors.size();i++){
             step(*motors[i],1);
         }
-        wait_us(100);
+        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(80);
+        wait_us(delayusOffset);
+}
+
+vector<string> splitString(string input){
+    istringstream iss(input);
+    vector<string> output;
+    while (iss) {
+        string word;
+        iss >> word;
+        output.push_back(word);
+    }
+    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 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));
+}
+
+//=================MAIN========================================
+int main() {
+    initialization();
+    setDirections(dirs, positiveDirs);
+    pc.attach(&callback);
+    
+    while(1) {
+        moveOneStep();
+        
+    }
+}