Diff: main.cpp

Revision:

30:c40b060795a2

Parent:

29:f888a2394027

Child:

31:5d6a97adae07

--- a/main.cpp	Wed Aug 07 12:30:41 2019 +0000
+++ b/main.cpp	Wed Sep 04 16:31:22 2019 +0000
@@ -20,18 +20,18 @@
   100000           //VREFA and VREFB PWM frequency (Hz)
 };
 
-Thread canrxa;
-Thread cantxa;
+Thread cantxa(osPriorityNormal);
+Thread canrxa(osPriorityNormal);
 
 // Utility
-InterruptIn button(USER_BUTTON);
+//InterruptIn button(USER_BUTTON);
 DigitalOut led(LED1);
 
 // Motor Control
 L6208 *motor;
 
-InterruptIn end0(PA_5, PullUp);
-InterruptIn end1(PA_6, PullUp);
+InterruptIn end1(USER_BUTTON, PullUp);
+DigitalIn end0(PA_5, PullUp);
 InterruptIn enc(PC_12, PullUp);
 
 typedef enum
@@ -60,9 +60,21 @@
   CAMERA2,
 }JOINT;
 
-long int pose, current_pose;
-int speed, current_speed;
-
+float pose, current_pose;
+float speed, current_speed;
+void zero()
+{
+    printf("zero");
+    motor->run(StepperMotor::BWD);
+    while(!end0){
+        }
+    motor->hard_stop();
+    motor->set_home();
+    motor->go_to(0);
+    printf("END0: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
+}
+    
+    
 uint32_t gen_can_id(CAN_COMMANDS message_id, JOINT can_id)
 {
   uint32_t id = (uint32_t)can_id;     // LSB byte is the controller id.
@@ -71,12 +83,12 @@
   return id;
 }
 
-int angle_deparse (long int pose, float offset)
+double angle_deparse (long int pose, float offset)
 {
   offset = offset * 0.00872664625;
-  float angle = pose * 0.00000425 ; //do something
+  double angle = pose *0.00000487012987; //do something 0,0004791666667
   angle = (angle - offset) * 100;
-  return (int)angle;
+  return angle;
 }
 void motor_error_handler(uint16_t error)
 {
@@ -84,33 +96,17 @@
 
 }
 
-void end0_int_handler()
-{
-  motor->hard_stop();
 
-  motor->run(StepperMotor::FWD);
-
-  printf("END0: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
-
-
-}
 
 void end1_int_handler()
 {
-  motor->hard_stop();
+ // motor->hard_stop();
 
-  motor->run(StepperMotor::BWD);
+  motor->run(StepperMotor::FWD);
 
   printf("END1: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
 }
 
-void motor_set_home()
-{
-  motor->hard_stop();
-  motor->set_home();
-  motor->go_to(0);
-
-}
 
 // CAN
 CAN can1(PB_8, PB_9);     // RX, TX
@@ -120,30 +116,40 @@
 
 void cantx ()
 {
+    while(1)
+    {
+    
+    int _pose;
     messageOut.format = CANExtended;
     messageOut.id=gen_can_id(JOINT_CURRENT_POSITION, BASE);
     pose=angle_deparse(motor->get_position(), 0);
-
-    messageOut.data[3]=pose;
-    messageOut.data[2]=pose >>8;
-    messageOut.data[1]=pose >>16;
-    messageOut.data[0]=pose >>24;
+    _pose=pose*100;
+    messageOut.data[3]=_pose;
+    messageOut.data[2]=_pose >>8;
+    messageOut.data[1]=_pose >>16;
+    messageOut.data[0]=_pose >>24;
 
     int status = can1.write(messageOut);
-    printf("CAN send CURRENT POSITION Joint status %d : pose %d",status, pose);
-
-    wait(1);
+    led=!status;
+    printf("CAN send CURRENT POSITION Joint status %d : pose %f\t\n",status, pose);
+    }
+    
 
 
 
 }
-
+void cantx_ISR()
+{
+    cantx();
+    osDelay(60);
+}
 
 
 void canrx()
 {
   while(1)
   {
+   // printf("receive\t\n");
     if(can1.read(messageIn)&&messageIn.id==gen_can_id(JOINT_SET_SPEED,BASE))
     {
       speed=messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24);
@@ -182,17 +188,22 @@
     
   }
 }
-
+void canrx_ISR()
+{
+    canrx();
+    osDelay(10);
+}
 
 /* Main ----------------------------------------------------------------------*/
 
 int main()
 {
+  
   can1.frequency(125000);
-
+   
   // Motor Initialization
   motor = new L6208(D2, D8, D7, D4, D5, D6, VREFA_PWM_PIN, VREFB_PWM_PIN);
-
+  motor->set_step_mode(StepperMotor::STEP_MODE_1_16);
   if (motor->init(&init) != COMPONENT_OK)
   {
     printf("ERROR: vvMotor Init\n\r");
@@ -201,20 +212,17 @@
 
   motor->attach_error_handler(&motor_error_handler);
 
-  end0.rise(&end0_int_handler);
-  end1.rise(&end1_int_handler);
-  end1.fall(&motor_set_home);
 
-
+  end1.rise(&end1_int_handler);
 
-  motor->set_step_mode(StepperMotor::STEP_MODE_1_16);
   printf("DONE: Motor Init\n\r");
 
   // CAN Initialization
-
-  canrxa.start(canrx);
-  cantxa.start(cantx);
-
+    zero();
+    
+  canrxa.start(canrx_ISR);
+  cantxa.start(cantx_ISR);
+  
   printf("DONE: CAN Init\n\r");