Diff: main.cpp

Revision:

29:93a31c16467b

Parent:

28:637b6f726971

Child:

30:62e21e5b4445

--- a/main.cpp	Fri Jun 21 07:56:33 2019 +0000
+++ b/main.cpp	Sat Sep 14 20:33:53 2019 +0000
@@ -1,20 +1,58 @@
 #include "mbed.h"
 #include "L6206.h"
+#include "BDCMotor.h"
+#include <math.h>
 
-#define JOINT_SET_SPEED 20
+/****************************/
+/*      PIN DEFINITION      */
+/****************************/
+
+//  CAN
+#define CAN_RX  PB_8
+#define CAN_TX  PB_9
+
+//  ENCODER
+#define CH_A    PA_8
+#define CH_B    PA_9
+
+/********************************/
+/*             CAN              */
+/********************************/
 
-#define JOINT_ID 3
+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;
+
+//
 
 static volatile uint16_t gLastError;
 static volatile uint8_t gStep = 0;
 
-int current_pose = 0;
-int speed = 0;
-
 L6206_init_t init =
 {
-    PARALLELING_NONE___1_BIDIR_MOTOR_BRIDGE_A__1_BIDIR_MOTOR_BRIDGE_B,
+    L6206_CONF_PARAM_PARALLE_BRIDGES,                                                    
     {L6206_CONF_PARAM_FREQ_PWM1A, L6206_CONF_PARAM_FREQ_PWM2A, L6206_CONF_PARAM_FREQ_PWM1B, L6206_CONF_PARAM_FREQ_PWM2B},
     {100,100,100,100},
     {FORWARD,BACKWARD,FORWARD,BACKWARD},
@@ -22,19 +60,18 @@
     {FALSE,FALSE}
 };
 
-L6206 *motor;
-
-Thread canrxa;
-
-//Utility
-DigitalOut led(LED1); //Change?
+//  Motor definition
+L6206 *LinAct;
+L6206 *EndEff;
 
-void motor_zero()
-{
-  motor->run(0, BDCMotor::FWD);
-  motor->run(1, BDCMotor::FWD);
-}
+int speed_elbow = 0;
+int speed_ee = 0;
 
+/*********************************/
+/*      Interrupt Handlers       */
+/*********************************/
+
+//  Error Handler (called by the library when it reports an error)
 void my_error_handler(uint16_t error)
 {
   /* Backup error number */
@@ -43,118 +80,151 @@
   /* Enter your own code here */
 }
 
+//  Flag Handler (overcurrent and thermal alarms reporting)
 void my_flag_irq_handler(void)
 {
-  /* Code to be customised */
-  /************************/
-  /* Get the state of bridge A */
-  uint16_t bridgeState  = motor->get_bridge_status(0);
-  
-  if (bridgeState == 0) {
-    if ((motor->get_device_state(0) != INACTIVE)||
-        (motor->get_device_state(1) != INACTIVE)) {
-      /* Bridge A was disabling due to overcurrent or over temperature */
-      /* When at least on of its  motor was running */
-        my_error_handler(0XBAD0);
+    /* Get the state of bridge A */
+    uint16_t bridgeState  = EndEff->get_bridge_status(0);
+
+    if (bridgeState == 0) 
+    {
+        if ((EndEff->get_device_state(0) != INACTIVE)||
+            (EndEff->get_device_state(1) != INACTIVE)) 
+        {
+            /* Bridge A was disabling due to overcurrent or over temperature */
+            /* When at least on of its  motor was running */
+            my_error_handler(0XBAD0);
+        }
     }
-  }
   
-  /* Get the state of bridge B */
-  bridgeState  = motor->get_bridge_status(1);
+    /* Get the state of bridge B */
+    bridgeState  = LinAct->get_bridge_status(1);
   
-  if (bridgeState == 0)  {
-    if ((motor->get_device_state(2) != INACTIVE)||
-        (motor->get_device_state(3) != INACTIVE)) {
-      /* Bridge A was disabling due to overcurrent or over temperature */
-      /* When at least on of its  motor was running */
-        my_error_handler(0XBAD1);
-    }
-  }  
+    if (bridgeState == 0)  
+    {
+        if ((LinAct->get_device_state(2) != INACTIVE)||
+            (LinAct->get_device_state(3) != INACTIVE)) 
+        {
+            /* Bridge A was disabling due to overcurrent or over temperature */
+            /* When at least on of its  motor was running */
+            my_error_handler(0XBAD1);
+        }
+    }  
 }
 
-// CAN, to revise
+/****************************/
+/*      CAN Interface       */
+/****************************/
+
 CAN can1(PB_8, PB_9);     // RX, TX
 
-CANMessage messageIn;
-CANMessage messageOut;
+Thread t_canrx, t_cantx;
 
-void canrx()
+uint32_t gen_can_id(CAN_COMMANDS message_id, JOINT can_id)
 {
-  while(1)
-  {    
-    if(can1.read(messageIn))
+    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;
+}
+
+bool can_rx()
+{
+    CANMessage messageIn;
+    messageIn.format = CANExtended;
+    bool status = can1.read(messageIn);
+    printf ("CAN ID : %d Message received : %d\n\r", messageIn.id, status);
+    
+    if(can1.read(messageIn) && messageIn.id == gen_can_id(JOINT_SET_SPEED, ELBOW))
     {
-      printf("READ!\n\r");
-      
-      if(messageIn.id == ((JOINT_SET_SPEED << 8) + JOINT_ID))
-      {
-          speed = 0;
-          speed = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]);
-          
-          motor->set_speed(0, speed);
-          (speed > 0) ? motor->run(0, BDCMotor::BWD) : motor->run(0, BDCMotor::FWD);
-          
-          printf("CAN: mess %d\n\r", speed);
-      }
+        speed_elbow = messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24);
+    }
+    
+    if(can1.read(messageIn) && messageIn.id == gen_can_id(JOINT_SET_SPEED, END_EFFECTOR))
+    {
+        speed_ee = messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24);
+    }
+    
+    if(can1.read(messageIn) && messageIn.id == gen_can_id(JOINT_ZERO,ELBOW))
+    {
+        if((messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24)) == 1)
+        {
+            LinAct->run(1, BDCMotor::BWD);
+        }
     }
     
-    int speed = 5000;
-    
-    messageOut.data[0] = speed >> 24;
-   messageOut.data[1] = speed >> 16;
-   messageOut.data[2] = speed >> 8;
-   messageOut.data[3] = speed;
-   
-   messageOut.id = (3 << 8) + 2;
-    int status = can1.write(messageOut);
-    
-    printf("STATUS: %d\n\r", status);
-    
-    wait(0.01);
-  }
+    return status;
 }
 
+void can_rx_isr()
+{
+    while(1)
+    {
+        can_rx();
+        osDelay(10);
+    }
+}
 
-/* Main ----------------------------------------------------------------------*/
+/*****************************/
+/*            MAIN           */
+/*****************************/
+
 int main()
 {
-  can1.frequency(125000);
-  messageIn.format=CANExtended;
-  messageOut.format=CANExtended;
-  
-  
-  // Motor Initialization 
-  
+    can1.frequency(125000);
+
+//  Motor Initialization   
 #ifdef TARGET_STM32F429
-    motor = new L6206(D2, A4, PB_4, PC_7, PA_15, PB_3);
+    LinAct = new L6206(PB_14, PB_15, PA_8, PA_9, PC_6, PC_7);     // EN_A, EN_B, IN1_A, IN2_A, IN1_B, IN2_B
+    EndEff = new L6206(PB_14, PB_15, PA_8, PA_9, PC_6, PC_7);     // EN_A, EN_B, IN1_A, IN2_A, IN1_B, IN2_B
 #else
-    motor = new L6206(D2, A4, D5, D4, A0, A1);
+    //LinAct = new L6206(PB_14, PB_15, PA_8, PA_9, PC_6, PC_7);
+    LinAct = new L6206(D2, A4, D5, D4, A0, A1);
+    EndEff = new L6206(D2, A4, D5, D4, A0, A1);
 #endif
- 
-  if (motor->init(&init) != COMPONENT_OK) 
-  {
-    printf("ERROR: vvMotor Init\n\r");
-    exit(EXIT_FAILURE);
-  }
 
-  motor->attach_flag_interrupt(my_flag_irq_handler);
-  motor->attach_error_handler(my_error_handler);
+    if (LinAct->init(&init) != COMPONENT_OK) 
+    {
+        printf("ERROR: vvMotor Init\n\r");
+        exit(EXIT_FAILURE);
+    }
+    if (EndEff->init(&init) != COMPONENT_OK) 
+    {
+        printf("ERROR: vvMotor Init\n\r");
+        exit(EXIT_FAILURE);
+    }
+    
+    LinAct->attach_flag_interrupt(my_flag_irq_handler);
+    LinAct->attach_error_handler(my_error_handler);
+    EndEff->attach_flag_interrupt(my_flag_irq_handler);
+    EndEff->attach_error_handler(my_error_handler);
+    printf("DONE: Motor Init\n\r");
+    
+    LinAct->set_dual_full_bridge_config(PARALLELING_NONE___1_BIDIR_MOTOR_BRIDGE_A__1_BIDIR_MOTOR_BRIDGE_B);
+    EndEff->set_dual_full_bridge_config(PARALLELING_NONE___1_BIDIR_MOTOR_BRIDGE_A__1_BIDIR_MOTOR_BRIDGE_B);
 
-  printf("DONE: Motor Init\n\r");
-  
-  // CAN Initialization
-  
-  canrxa.start(canrx);
-  
-  printf("DONE: CAN Init\n\r");
-  
-  //motor->set_speed(1, 50);
-  //motor->run(1, BDCMotor::FWD);
-  
-  printf("Running!\n\r");
-  
-  while(true)
-  {
-    wait(1);
-  }
-}
+    // CAN Initialization
+    t_canrx.start(can_rx_isr);
+    printf("DONE: CAN Init\n\r");
+
+    while(true)
+    {            
+        EndEff->set_speed(0, speed_ee);
+        if(speed_ee < 0)
+            EndEff->run(0, BDCMotor::BWD);
+        else if(speed_ee > 0)
+            EndEff->run(0, BDCMotor::FWD);
+        else if(speed_ee == 0)
+            EndEff->hard_stop(0);
+            
+        LinAct->set_speed(1, speed_elbow);
+        if(speed_elbow < 0)
+            LinAct->run(1, BDCMotor::BWD);
+        else if(speed_elbow > 0)
+            LinAct->run(1, BDCMotor::FWD);
+        else if(speed_elbow == 0)
+            LinAct->hard_stop(1);
+          
+        osDelay(100);
+    }
+}
\ No newline at end of file