Diff: main.cpp

Revision:

33:c1cefad6d338

Parent:

32:465e41868fe4

Child:

34:215d5ee0f434

--- a/main.cpp	Fri Sep 13 09:24:19 2019 +0000
+++ b/main.cpp	Sun Sep 15 08:47:18 2019 +0000
@@ -1,9 +1,10 @@
 #include "mbed.h"
 #include "L6208.h"
- 
+
 #define VREFA_PWM_PIN D3
 #define VREFB_PWM_PIN D9
-#define SEND_FREQUENCY 10 //Hz
+#define BAUDRATE 9600
+#define JOINT 2
 l6208_init_t init =
 {
   8000,            //Acceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
@@ -19,221 +20,134 @@
   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,
-}CAN_COMMANDS;
- 
-typedef enum
-{
-  BASE=1,
-  SHOULDER,
-  ELBOW,
-  WRIST1,
-  WRIST2,
-  WRIST3,
-  END_EFFECTOR,
-  CAMERA1,
-  CAMERA2,
-}JOINT;
- 
+Serial serial(PA_2, PA_3); 
+
+
+
 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());
+  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.
-  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;
-}
-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;
-}
- void motor_error_handler(uint16_t error)
+
+void motor_error_handler(uint16_t error)
 {
   printf("ERROR: Motor Runtime\n\r");
-
+  
 }
- 
+
 void end1_int_handler()
 {
- // motor->hard_stop();
- 
+  // 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 serialrx()
 {
-    while(1)
+
+  int id, speed;
+  
+  
+    if(serial.readable() ) 
     {
+      serial.scanf("%d %d", &id, &speed );
+      printf("%d %d\n", id, speed);
+      if (id==JOINT)
+      {
+        led=!led;
+        current_speed=speed;
+      }
     
-    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);
-    }
     
- 
- 
- 
+  }
 }
-void cantx_ISR()
-{
-    cantx();
-    osDelay(1/SEND_FREQUENCY*1000);
-}
- 
- 
-void canrx()
-{
-  while(1)
+  
+  void fmotor()
   {
-   // printf("receive\t\n");
-    if(can1.read(messageIn)&&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 %d\n\r", speed);
-      current_speed=speed;
- 
- 
+  
+      
       if (current_speed>0)
       {
-        motor->set_max_speed(current_speed*80);
-        motor->run(StepperMotor::FWD);
+        printf("run FWD\n");
+        motor->set_max_speed(abs(current_speed*80));
+        motor->run(StepperMotor::BWD);
       }
       else if (current_speed<0)
       {
-        motor->set_max_speed(current_speed*80);
-        motor->run(StepperMotor::BWD);
+        printf("run BWD\n");
+        motor->set_max_speed(abs(current_speed*80));
+        motor->run(StepperMotor::FWD);
       }
- 
+      
       else
       {
-        motor->soft_stop();
+        motor->hard_stop();
         current_pose= motor->get_position();
         motor->go_to(current_pose);
       }
-    }
-    
-    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()
-{
+  
+  
+  /* 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)
+  int main()
   {
-    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
+    led=1;
+    serial.baud(BAUDRATE);
+    
+    // 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);
     
     
-  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
+   // end1.rise(&end1_int_handler);
+    
+    printf("DONE: Motor Init\n\r");
+    
+    
+    printf("Running!\n\r");
+    
+    //zero();
+    
+    while(true)
+    {
+      serialrx();
+      //wait (0.001);
+      fmotor();
+      
+    }
+  }
\ No newline at end of file