Diff: main.cpp

Revision:

7:b5053ba85843

Parent:

6:e69f8c6faebd

Child:

8:dc3d3dfc777e

--- a/main.cpp	Sun Apr 02 06:06:39 2017 +0000
+++ b/main.cpp	Thu Apr 06 06:31:20 2017 +0000
@@ -13,7 +13,8 @@
 vector<int> currentPosition;
 vector<int> goalPosition;
 vector<int> buffer;
-bool negativeDirs [4] = {1,0,0,0};
+bool negativeDirs [4] = {1,1,0,0};
+bool positiveDirs [4] = {0,0,1,1};
 
 //====================SERIAL COMMUNICATION=============================
 Serial pc(USBTX, USBRX);
@@ -25,8 +26,8 @@
 int stepsY = 0;
 int stepsZ = 0;
 int stepsRot = 0;
-int mmToSteps = 1;
-int degToSteps = 1;
+int mmToSteps = 2400;
+int degToSteps = 4.444;
 DigitalOut led(LED1);
 //======================FUNCTIONS=======================================
 void step(DigitalOut stepsPin,int voltage)
@@ -130,14 +131,17 @@
 }
 
 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));
@@ -152,16 +156,18 @@
 {
     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++){
@@ -169,6 +175,11 @@
             }
             re_initialization();
             buffer.clear();
+            setDirections(dirs, positiveDirs);
+            while(motors.size()>0){
+                moveOneStep();
+                wait_us(1000);
+            }
         }
     }
 }
@@ -176,12 +187,15 @@
 //=================MAIN========================================
 int main()
 {
+    pc.baud(19200);
     initialization();
-//    DigitalOut led(LED1);
+    DigitalOut led(LED1);
     setDirections(dirs, negativeDirs);
     goHomeRotation();
     goHome();
     while(1) {
         serial_check();
+//        led = *switches[1];
+//        moveOneStep();
     }
 }