SHOULDER

Dependencies:   X-NUCLEO-IHM05A1

Files at this revision

API Documentation at this revision

Comitter:
stebonicelli
Date:
Fri Sep 13 20:40:50 2019 +0000
Parent:
33:48196cd5c052
Commit message:
Post dinner revision

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r 48196cd5c052 -r 4be6353f0186 main.cpp
--- a/main.cpp	Fri Sep 13 15:26:06 2019 +0000
+++ b/main.cpp	Fri Sep 13 20:40:50 2019 +0000
@@ -4,23 +4,24 @@
 #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
-  80,              //Acceleration current torque in % (from 0 to 100)
-  8000,            //Deceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
-  80,              //Deceleration current torque in % (from 0 to 100)
-  8000,            //Running speed in step/s or (1/16)th step/s for microstep modes
-  80,              //Running current torque in % (from 0 to 100)
-  40,              //Holding current torque in % (from 0 to 100)
-  STEP_MODE_1_16,  //Step mode via enum motorStepMode_t
-  FAST_DECAY,      //Decay mode via enum motorDecayMode_t
-  0,               //Dwelling time in ms
-  FALSE,           //Automatic HIZ STOP
-  100000           //VREFA and VREFB PWM frequency (Hz)
+    8000,            //Acceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
+    80,              //Acceleration current torque in % (from 0 to 100)
+    8000,            //Deceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
+    80,              //Deceleration current torque in % (from 0 to 100)
+    8000,            //Running speed in step/s or (1/16)th step/s for microstep modes
+    80,              //Running current torque in % (from 0 to 100)
+    40,              //Holding current torque in % (from 0 to 100)
+    STEP_MODE_1_16,  //Step mode via enum motorStepMode_t
+    FAST_DECAY,      //Decay mode via enum motorDecayMode_t
+    0,               //Dwelling time in ms
+    FALSE,           //Automatic HIZ STOP
+    100000           //VREFA and VREFB PWM frequency (Hz)
 };
+
 // CAN
-
 CAN can1(PA_11, PA_12);     // RX, TX
 
 CANMessage messageIn;
@@ -42,28 +43,28 @@
 
 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,
+    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;
 
 typedef enum
 {
-  BASE=1,
-  SHOULDER,
-  ELBOW,
-  WRIST1,
-  WRIST2,
-  WRIST3,
-  END_EFFECTOR,
-  CAMERA1,
-  CAMERA2,
+    BASE=1,
+    SHOULDER,
+    ELBOW,
+    WRIST1,
+    WRIST2,
+    WRIST3,
+    END_EFFECTOR,
+    CAMERA1,
+    CAMERA2,
 }JOINT;
 
 float pose, current_pose;
@@ -71,140 +72,141 @@
 
 void set_zero()
 {
-  printf("set zero\t\n");
-  motor->hard_stop();
-  motor->set_home();
-  motor->go_to(0);
+    printf("set zero\t\n");
+    
+    motor->hard_stop();
+    motor->set_home();
+    motor->go_to(0);
 }
+
 void zero()
 {
     printf("zero");
     motor->run(StepperMotor::FWD);
-    
 }
-    
-    
+
 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.
-  id |= (uint32_t)message_id << 8;  // Next lowest byte is the packet id.
-  id |= 0x80000000;              // Send in Extended Frame Format.
-  return id;
+    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;
 }
+
 double to_rad(double angle)
 {
-  return angle*0.0174533;
+    return angle*0.0174533;
 }
+
 double angle_deparse (long int pose, float offset)
 {
-  offset = offset * 0.00872664625;
-  double angle = pose *0.000487012987; //do something 0,0004791666667
-  angle = (angle - offset);
-  return angle;
+    offset = offset * 0.00872664625;
+    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");
+    printf("ERROR: Motor Runtime\n\r");
+}
 
-}
 void end0_int_handler()
 {
-  motor->hard_stop();
+    motor->hard_stop();
+    motor->run(StepperMotor::BWD);
+    
+    printf("END0: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
+}
 
-  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);
-
-  
+    motor->hard_stop();
+    int position = motor->get_position();
+    motor->go_to(position);
 }
 
 void end1_int_handler()
 {
-  motor->hard_stop();
-
-  motor->run(StepperMotor::FWD);
-
-  printf("END1: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
+    motor->hard_stop();
+    motor->run(StepperMotor::FWD);
+    
+    printf("END1: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
 }
 
-
-
 void cantx ()
 {
     while(1)
     {
-    
-    int _pose;
-    messageOut.format = CANExtended;
-    messageOut.id=gen_can_id(JOINT_CURRENT_POSITION, SHOULDER);
-    pose=angle_deparse(motor->get_position(), 0);
-    _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);
-    led=status;
-    printf("CAN send CURRENT POSITION Joint status %d : pose %f\t\n",status, pose);
+        int _pose;
+        
+        messageOut.format = CANExtended;
+        messageOut.id=gen_can_id(JOINT_CURRENT_POSITION, SHOULDER);
+        
+        pose=angle_deparse(motor->get_position(), 0);
+        _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 %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,SHOULDER))
-    {
-      speed=messageIn.data[3] + (messageIn.data[2] << 8) + (messageIn.data[1] << 16) + (messageIn.data[0] << 24);
-      printf("CAN: mess  received  : %f\n\r", speed);
-      current_speed=speed;
-
-      led = !led;
+    while(1)
+    {   
+        if(can1.read(messageIn))
+        {
+            led = !led;
+            
+            printf("Read!\n\r");
+            
+            if(messageIn.id==gen_can_id(JOINT_SET_SPEED,SHOULDER))
+            {
+                speed=messageIn.data[3] + (messageIn.data[2] << 8) + (messageIn.data[1] << 16) + (messageIn.data[0] << 24);
+                printf("CAN: mess  received  : %f\n\r", speed);
+                current_speed=speed;
+                
+                if (current_speed>0)
+                {
+                    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);
+                }
+            }
+            else if(messageIn.id==gen_can_id(JOINT_ZERO,SHOULDER))
+            {
+                if((messageIn.data[3] + (messageIn.data[2] << 8) + (messageIn.data[1] << 16) + (messageIn.data[0] << 24))==1)
+                {
+                    zero();
+                    motor->wait_while_active();
+                }
+            }
+        }
+    }
+}
 
-      if (current_speed>0)
-      {
-        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);
-      }
-    }
-    
-    if(can1.read(messageIn)&&messageIn.id==gen_can_id(JOINT_ZERO,SHOULDER))
-    {
-      if((messageIn.data[3] + (messageIn.data[2] << 8) + (messageIn.data[1] << 16) + (messageIn.data[0] << 24))==1)
-      {
-        zero();
-        motor->wait_while_active();
-      }
-    }
-    
-  }
-}
 void canrx_ISR()
 {
     canrx();
@@ -215,39 +217,42 @@
 
 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");
-    exit(EXIT_FAILURE);
-  }
-
-  motor->attach_error_handler(&motor_error_handler);
-
-  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();
+    
+    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");
+        exit(EXIT_FAILURE);
+    }
+    
+    motor->attach_error_handler(&motor_error_handler);
+    
+    end0.rise(&end0_int_handler);
+    end0.fall(&end0_int_handler_fall);
+    end1.rise(&end1_int_handler);
+    end1.fall(&set_zero);
     
-  canrxa.start(canrx_ISR);
-  //cantxa.start(cantx_ISR);
-  
-  printf("DONE: CAN Init\n\r");
-  
-  printf("Running!\n\r");
-  //zero();
-  while(true)
-  {
-    wait(1000);
-  }
+    printf("DONE: Motor Init\n\r");
+    
+    // CAN Initialization
+    // zero();
+    
+    canrxa.start(canrx_ISR);
+    //cantxa.start(cantx_ISR);
+    
+    printf("DONE: CAN Init\n\r");
+    
+    printf("Running!\n\r");
+    
+    //zero();
+    
+    while(true)
+    {
+        wait(1000);
+    }
 }