main.cpp

00001 #include "mbed.h"
00002 #include "L6206.h"
00003 #include "BDCMotor.h"
00004 #include <math.h>
00005 
00006 /****************************/
00007 /*      PIN DEFINITION      */
00008 /****************************/
00009 
00010 //  CAN
00011 #define CAN_RX  PB_8
00012 #define CAN_TX  PB_9
00013 
00014 //  ENCODER
00015 #define CH_A    PA_8
00016 #define CH_B    PA_9
00017 
00018 /********************************/
00019 /*             CAN              */
00020 /********************************/
00021 
00022 typedef enum
00023 {
00024     JOINT_SET_SPEED = 20,
00025     JOINT_SET_POSITION,
00026     JOINT_CURRENT_POSITION,
00027     JOINT_CURRENT_SPEED,
00028     JOINT_STATUS,
00029     JOINT_ERROR,
00030     JOINT_TORQUE,
00031     JOINT_MAXTORQUE,
00032     JOINT_ZERO,
00033 }   CAN_COMMANDS;
00034 
00035 typedef enum
00036 {
00037     BASE = 1,
00038     SHOULDER,
00039     ELBOW,
00040     WRIST1,
00041     WRIST2,
00042     WRIST3,
00043     END_EFFECTOR,
00044     CAMERA1,
00045     CAMERA2,
00046 }   JOINT;
00047 
00048 //
00049 DigitalOut myled(LED1);
00050 
00051 static volatile uint16_t gLastError;
00052 static volatile uint8_t gStep = 0;
00053 
00054 L6206_init_t init =
00055 {
00056     L6206_CONF_PARAM_PARALLE_BRIDGES,                                                    
00057     {L6206_CONF_PARAM_FREQ_PWM1A, L6206_CONF_PARAM_FREQ_PWM2A, L6206_CONF_PARAM_FREQ_PWM1B, L6206_CONF_PARAM_FREQ_PWM2B},
00058     {100,100,100,100},
00059     {FORWARD,BACKWARD,FORWARD,BACKWARD},
00060     {INACTIVE,INACTIVE,INACTIVE,INACTIVE},
00061     {FALSE,FALSE}
00062 };
00063 
00064 //  Motor definition
00065 L6206 *LinAct;
00066 L6206 *EndEff;
00067 
00068 int speed_elbow = 0;
00069 int speed_ee = 0;
00070 
00071 /*********************************/
00072 /*      Interrupt Handlers       */
00073 /*********************************/
00074 
00075 //  Error Handler (called by the library when it reports an error)
00076 void my_error_handler(uint16_t error)
00077 {
00078   /* Backup error number */
00079   gLastError = error;
00080   
00081   /* Enter your own code here */
00082 }
00083 
00084 //  Flag Handler (overcurrent and thermal alarms reporting)
00085 void my_flag_irq_handler(void)
00086 {
00087     /* Get the state of bridge A */
00088     uint16_t bridgeState  = EndEff->get_bridge_status(0);
00089 
00090     if (bridgeState == 0) 
00091     {
00092         if ((EndEff->get_device_state(0) != INACTIVE)||
00093             (EndEff->get_device_state(1) != INACTIVE)) 
00094         {
00095             /* Bridge A was disabling due to overcurrent or over temperature */
00096             /* When at least on of its  motor was running */
00097             my_error_handler(0XBAD0);
00098         }
00099     }
00100   
00101     /* Get the state of bridge B */
00102     bridgeState  = LinAct->get_bridge_status(1);
00103   
00104     if (bridgeState == 0)  
00105     {
00106         if ((LinAct->get_device_state(2) != INACTIVE)||
00107             (LinAct->get_device_state(3) != INACTIVE)) 
00108         {
00109             /* Bridge A was disabling due to overcurrent or over temperature */
00110             /* When at least on of its  motor was running */
00111             my_error_handler(0XBAD1);
00112         }
00113     }  
00114 }
00115 
00116 /****************************/
00117 /*      CAN Interface       */
00118 /****************************/
00119 
00120 CAN can1(PB_8, PB_9);     // RX, TX
00121 
00122 Thread t_canrx, t_cantx;
00123 
00124 uint32_t gen_can_id(CAN_COMMANDS message_id, JOINT can_id)
00125 {
00126     uint32_t id = (uint32_t)can_id;     // LSB byte is the controller id.
00127     id |= (uint32_t)message_id << 8;  // Next lowest byte is the packet id.
00128     id |= 0x80000000;              // Send in Extended Frame Format.
00129     return id;
00130 }
00131 
00132 bool can_rx()
00133 {
00134     CANMessage messageIn;
00135     messageIn.format = CANExtended;
00136     bool status = can1.read(messageIn);
00137     printf ("CAN ID : %d Message received : %d\n\r", messageIn.id, status);
00138     
00139     if(can1.read(messageIn))
00140     {
00141         myled = 1;
00142         wait(0.5);
00143         myled = 0;
00144         wait(0.5);
00145         if(messageIn.id == gen_can_id(JOINT_SET_SPEED, ELBOW))
00146         {
00147             speed_elbow = messageIn.data[3] + (messageIn.data[2] << 8) + (messageIn.data[1] << 16) + (messageIn.data[0] << 24);
00148         }
00149     }
00150     
00151     /*
00152     if(can1.read(messageIn))
00153     {
00154         myled = 1;
00155         wait(0.5);
00156         myled = 0;
00157         wait(0.5);
00158         if(messageIn.id == gen_can_id(JOINT_SET_SPEED, END_EFFECTOR))
00159         {
00160             speed_ee = messageIn.data[3] + (messageIn.data[2] << 8) + (messageIn.data[1] << 16) + (messageIn.data[0] << 24);
00161         }
00162     }
00163     */
00164     
00165     if(can1.read(messageIn) && messageIn.id == gen_can_id(JOINT_ZERO,ELBOW))
00166     {
00167         if((messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24)) == 1)
00168         {
00169             LinAct->run(1, BDCMotor::BWD);
00170         }
00171     }
00172     
00173     return status;
00174 }
00175 
00176 void can_rx_isr()
00177 {
00178     while(1)
00179     {
00180         can_rx();
00181         osDelay(10);
00182     }
00183 }
00184 
00185 /*****************************/
00186 /*            MAIN           */
00187 /*****************************/
00188 
00189 int main()
00190 {
00191     can1.frequency(125000);
00192 
00193 //  Motor Initialization   
00194 #ifdef TARGET_STM32F429
00195     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
00196     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
00197 #else
00198     //LinAct = new L6206(PB_14, PB_15, PA_8, PA_9, PC_6, PC_7);
00199     LinAct = new L6206(D2, A4, D5, D4, A0, A1);
00200     EndEff = new L6206(D2, A4, D5, D4, A0, A1);
00201 #endif
00202 
00203     if (LinAct->init(&init) != COMPONENT_OK) 
00204     {
00205         printf("ERROR: vvMotor Init\n\r");
00206         exit(EXIT_FAILURE);
00207     }
00208     if (EndEff->init(&init) != COMPONENT_OK) 
00209     {
00210         printf("ERROR: vvMotor Init\n\r");
00211         exit(EXIT_FAILURE);
00212     }
00213     
00214     LinAct->attach_flag_interrupt(my_flag_irq_handler);
00215     LinAct->attach_error_handler(my_error_handler);
00216     EndEff->attach_flag_interrupt(my_flag_irq_handler);
00217     EndEff->attach_error_handler(my_error_handler);
00218     printf("DONE: Motor Init\n\r");
00219     
00220     LinAct->set_dual_full_bridge_config(PARALLELING_NONE___1_BIDIR_MOTOR_BRIDGE_A__1_BIDIR_MOTOR_BRIDGE_B);
00221     EndEff->set_dual_full_bridge_config(PARALLELING_NONE___1_BIDIR_MOTOR_BRIDGE_A__1_BIDIR_MOTOR_BRIDGE_B);
00222 
00223     // CAN Initialization
00224     t_canrx.start(can_rx_isr);
00225     printf("DONE: CAN Init\n\r");
00226 
00227     while(true)
00228     {            
00229         EndEff->set_speed(0, speed_ee);
00230         if(speed_ee < 0)
00231             EndEff->run(0, BDCMotor::BWD);
00232         else if(speed_ee > 0)
00233             EndEff->run(0, BDCMotor::FWD);
00234         else if(speed_ee == 0)
00235             EndEff->hard_stop(0);
00236             
00237         LinAct->set_speed(1, speed_elbow);
00238         if(speed_elbow < 0)
00239             LinAct->run(1, BDCMotor::BWD);
00240         else if(speed_elbow > 0)
00241             LinAct->run(1, BDCMotor::FWD);
00242         else if(speed_elbow == 0)
00243             LinAct->hard_stop(1);
00244           
00245         osDelay(100);
00246     }
00247 }