Diff: main.cpp

Revision:

32:03c98e297a4a

Parent:

31:f24535e65dae

Child:

33:48196cd5c052

--- a/main.cpp	Thu Sep 05 20:07:59 2019 +0000
+++ b/main.cpp	Fri Sep 13 09:26:41 2019 +0000
@@ -19,6 +19,12 @@
   FALSE,           //Automatic HIZ STOP
   100000           //VREFA and VREFB PWM frequency (Hz)
 };
+// CAN
+
+CAN can1(PA_11, PA_12);     // RX, TX
+
+CANMessage messageIn;
+CANMessage messageOut;
 
 Thread cantxa(osPriorityNormal);
 Thread canrxa(osPriorityNormal);
@@ -32,7 +38,7 @@
 
 InterruptIn end0(PA_5, PullUp);
 InterruptIn end1(PA_6, PullUp);
-InterruptIn enc(PC_12, PullUp);
+//InterruptIn enc(PC_12, PullUp);
 
 typedef enum
 {
@@ -73,7 +79,7 @@
 void zero()
 {
     printf("zero");
-    motor->run(StepperMotor::BWD);
+    motor->run(StepperMotor::FWD);
     
 }
     
@@ -105,26 +111,29 @@
 {
   motor->hard_stop();
 
-  motor->run(StepperMotor::FWD);
+  motor->run(StepperMotor::BWD);
 
   printf("END0: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
 }
+void end0_int_handler_fall()
+{
+  motor->hard_stop();
+  int position = motor->get_position();
+  motor->go_to(position);
+
+  
+}
 
 void end1_int_handler()
 {
   motor->hard_stop();
 
-  motor->run(StepperMotor::BWD);
+  motor->run(StepperMotor::FWD);
 
   printf("END1: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
 }
 
 
-// CAN
-CAN can1(PB_8, PB_9);     // RX, TX
-
-CANMessage messageIn;
-CANMessage messageOut;
 
 void cantx ()
 {
@@ -142,7 +151,7 @@
     messageOut.data[0]=_pose >>24;
 
     int status = can1.write(messageOut);
-    led=!status;
+    led=status;
     printf("CAN send CURRENT POSITION Joint status %d : pose %f\t\n",status, pose);
     }
     
@@ -164,9 +173,9 @@
    
     if(can1.read(messageIn)&&messageIn.id==gen_can_id(JOINT_SET_SPEED,SHOULDER))
     {
-      speed=messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24);
+      speed=messageIn.data[3] + (messageIn.data[2] << 8) + (messageIn.data[1] << 16) + (messageIn.data[0] << 24);
       printf("CAN: mess  received  : %d\n\r", speed);
-      current_speed=speed-100;
+      current_speed=speed;
 
 
       if (current_speed>0)
@@ -190,7 +199,7 @@
     
     if(can1.read(messageIn)&&messageIn.id==gen_can_id(JOINT_ZERO,SHOULDER))
     {
-      if((messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24))==1)
+      if((messageIn.data[3] + (messageIn.data[2] << 8) + (messageIn.data[1] << 16) + (messageIn.data[0] << 24))==1)
       {
         zero();
         motor->wait_while_active();
@@ -225,16 +234,18 @@
 
 
   end0.rise(&end0_int_handler);
+  end0.fall(&end0_int_handler_fall);
   end1.rise(&end1_int_handler);
   end1.fall(&set_zero);
 
   printf("DONE: Motor Init\n\r");
 
   // CAN Initialization
-    zero();
+   // zero();
+    
     
   canrxa.start(canrx_ISR);
-  cantxa.start(cantx_ISR);
+  //cantxa.start(cantx_ISR);
   
   printf("DONE: CAN Init\n\r");