Diff: main.cpp

Revision:

28:8878dd50b7e1

Parent:

27:275ba9c137c9

Child:

29:f888a2394027

--- a/main.cpp	Fri Jun 21 07:44:12 2019 +0000
+++ b/main.cpp	Fri Jun 21 07:58:24 2019 +0000
@@ -47,7 +47,7 @@
 
 void motor_zero()
 {
-    motor->run(StepperMotor::FWD);
+    motor->run(StepperMotor::BWD);
 }
 
 void button_int_handler()
@@ -56,13 +56,18 @@
     motor_zero();
 }
 
-void end0_int_handler()
+void end0_pressed()
 {
     motor->run(StepperMotor::BWD);
     printf("END0: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
 }
 
-void end1_int_handler()
+void end0_released()
+{
+    motor->go_to(motor->get_position());
+}
+
+void end1_pressed()
 {
     motor->hard_stop();
     motor->run(StepperMotor::FWD);
@@ -89,7 +94,7 @@
   {    
     if(can1.read(messageIn))
     {
-      printf("received\r\n");
+      printf("CAN: message received\r\n");
       if(messageIn.id == ((JOINT_SET_SPEED << 8) + JOINT_ID))
       {
           speed = 0;
@@ -98,11 +103,11 @@
           motor->set_max_speed(speed);
           (speed > 0) ? motor->run(StepperMotor::BWD) : motor->run(StepperMotor::FWD);
           
-          printf("CAN: mess %d\n\r", speed);
+          printf("CAN: setting speed -> %d\n\r", speed);
       }
     }
     
-    wait(0.01);
+    wait(0.05);
   }
 }
 
@@ -125,13 +130,19 @@
 
   motor->attach_error_handler(&motor_error_handler);
   
-  end0.rise(&end0_int_handler);
-  end1.rise(&end1_int_handler);
+  // Limit EndStop
+  end0.rise(&end0_pressed);
+  end0.fall(&end0_released);
+  // Zero EndStop
+  end1.rise(&end1_pressed);
   end1.fall(&motor_set_home);
   
   button.rise(&button_int_handler);
   
   motor->set_step_mode(StepperMotor::STEP_MODE_1_16);
+  
+  motor_set_home();
+  
   printf("DONE: Motor Init\n\r");
   
   // CAN Initialization