Diff: main.cpp

Revision:

31:f24535e65dae

Parent:

29:f888a2394027

Child:

32:03c98e297a4a

--- a/main.cpp	Wed Aug 07 12:30:41 2019 +0000
+++ b/main.cpp	Thu Sep 05 20:07:59 2019 +0000
@@ -3,7 +3,7 @@
 
 #define VREFA_PWM_PIN D3
 #define VREFB_PWM_PIN D9
-
+#define SEND_FREQUENCY 10 //Hz
 l6208_init_t init =
 {
   8000,            //Acceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
@@ -20,11 +20,11 @@
   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
@@ -60,9 +60,24 @@
   CAMERA2,
 }JOINT;
 
-long int pose, current_pose;
-int speed, current_speed;
+float pose, current_pose;
+float speed, current_speed;
 
+void set_zero()
+{
+  printf("set zero\t\n");
+  motor->hard_stop();
+  motor->set_home();
+  motor->go_to(0);
+}
+void zero()
+{
+    printf("zero");
+    motor->run(StepperMotor::BWD);
+    
+}
+    
+    
 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.
@@ -70,20 +85,22 @@
   id |= 0x80000000;              // Send in Extended Frame Format.
   return id;
 }
-
-int angle_deparse (long int pose, float offset)
+double to_rad(double angle)
+{
+  return angle*0.0174533;
+}
+double angle_deparse (long int pose, float offset)
 {
   offset = offset * 0.00872664625;
-  float angle = pose * 0.00000425 ; //do something
-  angle = (angle - offset) * 100;
-  return (int)angle;
+  double angle = pose *0.000487012987; //do something 0,0004791666667
+  angle = (angle - offset);
+  return angle;
 }
 void motor_error_handler(uint16_t error)
 {
   printf("ERROR: Motor Runtime\n\r");
 
 }
-
 void end0_int_handler()
 {
   motor->hard_stop();
@@ -91,8 +108,6 @@
   motor->run(StepperMotor::FWD);
 
   printf("END0: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
-
-
 }
 
 void end1_int_handler()
@@ -104,13 +119,6 @@
   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,34 +128,44 @@
 
 void cantx ()
 {
+    while(1)
+    {
+    
+    int _pose;
     messageOut.format = CANExtended;
-    messageOut.id=gen_can_id(JOINT_CURRENT_POSITION, BASE);
+    messageOut.id=gen_can_id(JOINT_CURRENT_POSITION, SHOULDER);
     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(1/SEND_FREQUENCY*1000);
+}
 
 
 void canrx()
 {
   while(1)
   {
-    if(can1.read(messageIn)&&messageIn.id==gen_can_id(JOINT_SET_SPEED,BASE))
+   
+    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);
-      printf("CAN: mess %d\n\r", speed);
+      printf("CAN: mess  received  : %d\n\r", speed);
       current_speed=speed-100;
 
 
@@ -170,29 +188,33 @@
       }
     }
     
-    if(can1.read(messageIn)&&messageIn.id==gen_can_id(JOINT_ZERO,BASE))
+    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)
       {
-        motor->hard_stop();
-        motor->set_max_speed(5000);
-        motor->run(StepperMotor::BWD);
+        zero();
+        motor->wait_while_active();
       }
     }
     
   }
 }
-
+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,26 +223,25 @@
 
   motor->attach_error_handler(&motor_error_handler);
 
+
   end0.rise(&end0_int_handler);
   end1.rise(&end1_int_handler);
-  end1.fall(&motor_set_home);
-
+  end1.fall(&set_zero);
 
-
-  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");
-
+  
 
 
   printf("Running!\n\r");
-
+  //zero();
   while(true)
   {
     wait(1000);