Revision 1:6cca05643958, committed 2019-10-04

Comitter:

edoardoVacchetto

Date:

Fri Oct 04 15:10:46 2019 +0000

Parent:

0:2c5d9f3acfb8

Commit message:

ttf

Changed in this revision

--- a/main.cpp	Thu Oct 03 17:29:28 2019 +0000
+++ b/main.cpp	Fri Oct 04 15:10:46 2019 +0000
@@ -35,7 +35,7 @@
 volatile int rxBufferPtr;
 
 float pose, current_pose;
-int8_t speed, current_speed;
+int speed, current_speed;
 
 void motor_error_handler(uint16_t error);   
 void zero();
@@ -66,7 +66,7 @@
         
     motor->set_home();
 
-    s_rx.printf("19 1\n");
+    s_rx.printf("19 1\r\n");
 
     timeOuter.start();
     
@@ -76,10 +76,11 @@
       if (timeOuter.read_ms() > 2000) {
           current_speed = 0;
           timeOuter.reset();
+          motor->hard_stop();
           Pc_Stat.printf("Command timeout!!\n");
       }
       //wait (0.001);
-      if (!posMode) fmotor();
+      //if (!posMode) fmotor();
       
     }
 }
@@ -124,14 +125,22 @@
   Pc_Stat.printf("ZERO FAILURE!\n");
 }  
 
-void fmotor()
+void fmotor(int speed)
 { 
-    unsigned int spd = abs(current_speed*80);
+    unsigned int sp = abs(speed*80);
     
-    if (spd) {
-        motor->set_max_speed(spd);
-        motor->run(current_speed >= 0 ? 
-            StepperMotor::FWD : StepperMotor::BWD);
+    if (sp) {
+        motor->set_max_speed(sp);
+        
+        if(speed > 0){
+            motor->run(StepperMotor::FWD );
+        } 
+        else if (speed < 0){
+            motor->run(StepperMotor::BWD );
+        }
+        
+        //motor->run(current_speed >= 0 ? 
+          //  StepperMotor::FWD : StepperMotor::BWD);
     } else {
         motor->hard_stop();
         current_pose = motor->get_position();
@@ -147,7 +156,7 @@
     timeOuter.reset();
           
     sscanf(dataRxBuffer, "%d %d", &joint, &spd);
-    //Pc_Stat.printf("joint: %d, value: %d", joint, spd);
+    Pc_Stat.printf("joint: %d, value: %d\n", joint, spd);
     posMode = false;
     
     switch(joint){
@@ -156,22 +165,23 @@
             break;
         case 11:
             posMode = true;
-            Pc_Stat.printf("Moving(position)... ");
+            Pc_Stat.printf("Moving(position)... \n");
             motor->set_max_speed(8000);
             motor->go_to(spd);
             Pc_Stat.printf("Done!\n");
             break;
         case 12:
-            Pc_Stat.printf("Moving(velocity)... ");
-            current_speed = spd;
+            Pc_Stat.printf("Moving(velocity)... \n");
+            fmotor(spd);
             s_rx.printf("15 %d\n",motor->get_position());
             break;
         case 13:
+            posMode = true;
             motor->hard_stop();
             Pc_Stat.printf("STOP.\n");
             break;           
         default:
-            Pc_Stat.printf("Unknown index %d - data: %d", joint, spd);
+            Pc_Stat.printf("Unknown index %d - data: %d\n", joint, spd);
             break;
     }
 }
@@ -181,7 +191,7 @@
     while(s_rx.readable()) 
     {
       char c = s_rx.getc();
-      Pc_Stat.printf("%c",c);
+      //Pc_Stat.printf("%c",c);
       
       if (c == '\n' || rxBufferPtr >= Rx_Buff_Dim - 1) {
           dataRxBuffer[rxBufferPtr] = '\0';