Diff: main.cpp

Revision:

29:f888a2394027

Parent:

28:8878dd50b7e1

Child:

30:c40b060795a2

Child:

31:f24535e65dae

--- a/main.cpp	Fri Jun 21 07:58:24 2019 +0000
+++ b/main.cpp	Wed Aug 07 12:30:41 2019 +0000
@@ -4,10 +4,6 @@
 #define VREFA_PWM_PIN D3
 #define VREFB_PWM_PIN D9
 
-#define JOINT_SET_SPEED 20
-
-#define JOINT_ID 2
-
 l6208_init_t init =
 {
   8000,            //Acceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
@@ -25,6 +21,7 @@
 };
 
 Thread canrxa;
+Thread cantxa;
 
 // Utility
 InterruptIn button(USER_BUTTON);
@@ -33,46 +30,78 @@
 // Motor Control
 L6208 *motor;
 
-InterruptIn end0(PC_4, PullUp);
-InterruptIn end1(PC_8, PullUp);
+InterruptIn end0(PA_5, PullUp);
+InterruptIn end1(PA_6, PullUp);
 InterruptIn enc(PC_12, PullUp);
 
-int32_t speed = 0;
+typedef enum
+{
+  JOINT_SET_SPEED = 20,
+  JOINT_SET_POSITION,
+  JOINT_CURRENT_POSITION,
+  JOINT_CURRENT_SPEED,
+  JOINT_STATUS,
+  JOINT_ERROR,
+  JOINT_TORQUE,
+  JOINT_MAXTORQUE,
+  JOINT_ZERO,
+}CAN_COMMANDS;
 
-void motor_error_handler(uint16_t error)
+typedef enum
 {
-    printf("ERROR: Motor Runtime\n\r");
-    while(1){} 
-}
+  BASE=1,
+  SHOULDER,
+  ELBOW,
+  WRIST1,
+  WRIST2,
+  WRIST3,
+  END_EFFECTOR,
+  CAMERA1,
+  CAMERA2,
+}JOINT;
 
-void motor_zero()
+long int pose, current_pose;
+int speed, current_speed;
+
+uint32_t gen_can_id(CAN_COMMANDS message_id, JOINT can_id)
 {
-    motor->run(StepperMotor::BWD);
+  uint32_t id = (uint32_t)can_id;     // LSB byte is the controller id.
+  id |= (uint32_t)message_id << 8;  // Next lowest byte is the packet id.
+  id |= 0x80000000;              // Send in Extended Frame Format.
+  return id;
 }
 
-void button_int_handler()
+int angle_deparse (long int pose, float offset)
 {
-    printf("POSITION: %d\n\r", motor->get_position());
-    motor_zero();
+  offset = offset * 0.00872664625;
+  float angle = pose * 0.00000425 ; //do something
+  angle = (angle - offset) * 100;
+  return (int)angle;
 }
-
-void end0_pressed()
+void motor_error_handler(uint16_t error)
 {
-    motor->run(StepperMotor::BWD);
-    printf("END0: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
+  printf("ERROR: Motor Runtime\n\r");
+
 }
 
-void end0_released()
+void end0_int_handler()
 {
-    motor->go_to(motor->get_position());
+  motor->hard_stop();
+
+  motor->run(StepperMotor::FWD);
+
+  printf("END0: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
+
+
 }
 
-void end1_pressed()
+void end1_int_handler()
 {
-    motor->hard_stop();
-    motor->run(StepperMotor::FWD);
+  motor->hard_stop();
 
-    printf("END1: Pressed\n\r");
+  motor->run(StepperMotor::BWD);
+
+  printf("END1: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
 }
 
 void motor_set_home()
@@ -80,6 +109,7 @@
   motor->hard_stop();
   motor->set_home();
   motor->go_to(0);
+
 }
 
 // CAN
@@ -88,26 +118,68 @@
 CANMessage messageIn;
 CANMessage messageOut;
 
+void cantx ()
+{
+    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;
+
+    int status = can1.write(messageOut);
+    printf("CAN send CURRENT POSITION Joint status %d : pose %d",status, pose);
+
+    wait(1);
+
+
+
+}
+
+
+
 void canrx()
 {
   while(1)
-  {    
-    if(can1.read(messageIn))
+  {
+    if(can1.read(messageIn)&&messageIn.id==gen_can_id(JOINT_SET_SPEED,BASE))
     {
-      printf("CAN: message received\r\n");
-      if(messageIn.id == ((JOINT_SET_SPEED << 8) + JOINT_ID))
+      speed=messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24);
+      printf("CAN: mess %d\n\r", speed);
+      current_speed=speed-100;
+
+
+      if (current_speed>0)
       {
-          speed = 0;
-          speed = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]);
-          
-          motor->set_max_speed(speed);
-          (speed > 0) ? motor->run(StepperMotor::BWD) : motor->run(StepperMotor::FWD);
-          
-          printf("CAN: setting speed -> %d\n\r", speed);
+        motor->set_max_speed(current_speed*80);
+        motor->run(StepperMotor::FWD);
+      }
+      else if (current_speed<0)
+      {
+        motor->set_max_speed(current_speed*80);
+        motor->run(StepperMotor::BWD);
+      }
+
+      else
+      {
+        motor->soft_stop();
+        current_pose= motor->get_position();
+        motor->go_to(current_pose);
       }
     }
     
-    wait(0.05);
+    if(can1.read(messageIn)&&messageIn.id==gen_can_id(JOINT_ZERO,BASE))
+    {
+      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);
+      }
+    }
+    
   }
 }
 
@@ -117,11 +189,10 @@
 int main()
 {
   can1.frequency(125000);
-  messageIn.format=CANExtended;
-  
-  // Motor Initialization 
+
+  // Motor Initialization
   motor = new L6208(D2, D8, D7, D4, D5, D6, VREFA_PWM_PIN, VREFB_PWM_PIN);
-  
+
   if (motor->init(&init) != COMPONENT_OK)
   {
     printf("ERROR: vvMotor Init\n\r");
@@ -129,36 +200,29 @@
   }
 
   motor->attach_error_handler(&motor_error_handler);
-  
-  // Limit EndStop
-  end0.rise(&end0_pressed);
-  end0.fall(&end0_released);
-  // Zero EndStop
-  end1.rise(&end1_pressed);
+
+  end0.rise(&end0_int_handler);
+  end1.rise(&end1_int_handler);
   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
-  
+
   canrxa.start(canrx);
-  
+  cantxa.start(cantx);
+
   printf("DONE: CAN Init\n\r");
-  
+
+
+
   printf("Running!\n\r");
-  
-  // DEBUG
-  //motor->set_max_speed(8000);
-  //motor->run(StepperMotor::FWD);
-  
+
   while(true)
   {
-    wait(1);
+    wait(1000);
   }
 }