Revision 35:87a2266254e5, committed 2019-09-13

Comitter:

stebonicelli

Date:

Fri Sep 13 20:34:45 2019 +0000

Parent:

34:7710cb1766e0

Commit message:

Post dinner revision

Changed in this revision

--- a/main.cpp	Fri Sep 13 15:22:17 2019 +0000
+++ b/main.cpp	Fri Sep 13 20:34:45 2019 +0000
@@ -1,235 +1,241 @@
 #include "mbed.h"
 #include "L6208.h"
- 
+
 #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)
 };
- 
+
 Thread cantxa(osPriorityNormal);
 Thread canrxa(osPriorityNormal);
- 
+
 // Utility
 //InterruptIn button(USER_BUTTON);
 DigitalOut led(LED1);
- 
+
 // Motor Control
 L6208 *motor;
- 
+
 InterruptIn end1(USER_BUTTON, PullUp);
 DigitalIn end0(PA_5, PullUp);
 InterruptIn enc(PC_12, PullUp);
- 
+
 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;
 float speed, current_speed;
+
 void zero()
 {
     printf("zero");
+    
     motor->run(StepperMotor::BWD);
-    while(!end0){
-        }
+    
+    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.
-  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)
+
+void motor_error_handler(uint16_t error)
 {
-  printf("ERROR: Motor Runtime\n\r");
+    printf("ERROR: Motor Runtime\n\r");
+}
 
-}
- 
 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());
 }
- 
- 
+
 // CAN
 CAN can1(PB_8, PB_9);     // RX, TX
- 
+
 CANMessage messageIn;
 CANMessage messageOut;
- 
-void cantx ()
+
+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);
-    _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, BASE);
+        
+        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)
-  {
-   // printf("receive\t\n");
-    if(can1.read(messageIn)&&messageIn.id==gen_can_id(JOINT_SET_SPEED,BASE))
+    while(1)
     {
-      speed=messageIn.data[3] + (messageIn.data[2] << 8) + (messageIn.data[1] << 16) + (messageIn.data[0] << 24);
-      printf("CAN: mess %f\n\r", speed);
-      current_speed=speed;
- 
-      led = !led;
- 
-      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))
+        {
+            led = !led;
+            
+            if(messageIn.id==gen_can_id(JOINT_SET_SPEED,BASE))
+            {
+                speed=messageIn.data[3] + (messageIn.data[2] << 8) + (messageIn.data[1] << 16) + (messageIn.data[0] << 24);
+                printf("CAN: mess %f\n\r", speed);
+                current_speed=speed;
+                
+                led = !led;
+                
+                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,BASE))
+            {
+                if((messageIn.data[3] + (messageIn.data[2] << 8) + (messageIn.data[1] << 16) + (messageIn.data[0] << 24))==1)
+                {
+                    zero();
+                    motor->wait_while_active();
+                }
+            }
+        }
     }
-    
-    if(can1.read(messageIn)&&messageIn.id==gen_can_id(JOINT_ZERO,BASE))
-    {
-      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();
     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");
-    exit(EXIT_FAILURE);
-  }
- 
-  motor->attach_error_handler(&motor_error_handler);
- 
-  end1.rise(&end1_int_handler);
- 
-  printf("DONE: Motor Init\n\r");
- 
-  // CAN Initialization
-  canrxa.start(canrx_ISR);
-  //cantxa.start(cantx_ISR);
-  
-  printf("DONE: CAN Init\n\r");
-  
-  printf("Running!\n\r");
-  
-  //zero();
-  
-  while(true)
-  {
-    wait(1000);
-  }
+    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);
+    
+    end1.rise(&end1_int_handler);
+    
+    printf("DONE: Motor Init\n\r");
+    
+    // CAN Initialization
+    canrxa.start(canrx_ISR);
+    //cantxa.start(cantx_ISR);
+    
+    printf("DONE: CAN Init\n\r");
+    
+    printf("Running!\n\r");
+    
+    //zero();
+    
+    while(true)
+    {
+        wait(1000);
+    }
 }
\ No newline at end of file