Diff: main.cpp

Revision:

20:8e5dd30b1b59

Parent:

19:9680ebe86f4a

Child:

21:533d014f09e0

--- a/main.cpp	Fri Feb 08 15:27:17 2019 +0000
+++ b/main.cpp	Fri Feb 08 19:48:07 2019 +0000
@@ -91,8 +91,29 @@
   {
     if(can1.read(messageIn, filter))
     {
-      printf("CAN: mess %d\n\r", (messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24)));
+      pose=messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24);
+      printf("CAN: mess %d\n\r", pose);
       printf("CANaacc: id %x \n\r ",messageIn.id);
+        if (pose == 0) 
+        {
+            motor->run(StepperMotor::BWD);
+        }
+        else if (pose == 50)
+        {
+            motor->soft_stop();
+            current_pose= motor->get_position();
+            motor->go_to(current_pose);
+        }
+        else if (pose == 100)
+        {
+            motor->run(StepperMotor::FWD);
+        }
+        else 
+        {
+            motor->soft_stop();
+            current_pose= motor->get_position();
+            motor->go_to(current_pose);
+        }
     }
   }
 }
@@ -118,7 +139,7 @@
   end1.fall(&motor_set_home);
   
   button.rise(&button_int_handler);
-  
+  motor->set_step_mode(StepperMotor::STEP_MODE_1_16);
   printf("DONE: Motor Init\n\r");
   
   // CAN Initialization
@@ -126,7 +147,7 @@
   
   printf("DONE: CAN Init\n\r");
   
-  motor->run(StepperMotor::FWD);
+
   
   printf("Running!\n\r");