main.cpp

00001 #include "mbed.h"
00002 #include "L6206.h"
00003 
00004 #define JOINT_SET_SPEED 20
00005 
00006 #define JOINT_ID 3
00007 
00008 
00009 static volatile uint16_t gLastError;
00010 static volatile uint8_t gStep = 0;
00011 
00012 int current_pose = 0;
00013 int speed = 0;
00014 
00015 L6206_init_t init =
00016 {
00017     PARALLELING_NONE___1_BIDIR_MOTOR_BRIDGE_A__1_BIDIR_MOTOR_BRIDGE_B,
00018     {L6206_CONF_PARAM_FREQ_PWM1A, L6206_CONF_PARAM_FREQ_PWM2A, L6206_CONF_PARAM_FREQ_PWM1B, L6206_CONF_PARAM_FREQ_PWM2B},
00019     {100,100,100,100},
00020     {FORWARD,BACKWARD,FORWARD,BACKWARD},
00021     {INACTIVE,INACTIVE,INACTIVE,INACTIVE},
00022     {FALSE,FALSE}
00023 };
00024 
00025 L6206 *motor;
00026 
00027 Thread canrxa;
00028 
00029 //Utility
00030 DigitalOut led(LED1); //Change?
00031 
00032 void motor_zero()
00033 {
00034   motor->run(0, BDCMotor::FWD);
00035   motor->run(1, BDCMotor::FWD);
00036 }
00037 
00038 void my_error_handler(uint16_t error)
00039 {
00040   /* Backup error number */
00041   gLastError = error;
00042   
00043   /* Enter your own code here */
00044 }
00045 
00046 void my_flag_irq_handler(void)
00047 {
00048   /* Code to be customised */
00049   /************************/
00050   /* Get the state of bridge A */
00051   uint16_t bridgeState  = motor->get_bridge_status(0);
00052   
00053   if (bridgeState == 0) {
00054     if ((motor->get_device_state(0) != INACTIVE)||
00055         (motor->get_device_state(1) != INACTIVE)) {
00056       /* Bridge A was disabling due to overcurrent or over temperature */
00057       /* When at least on of its  motor was running */
00058         my_error_handler(0XBAD0);
00059     }
00060   }
00061   
00062   /* Get the state of bridge B */
00063   bridgeState  = motor->get_bridge_status(1);
00064   
00065   if (bridgeState == 0)  {
00066     if ((motor->get_device_state(2) != INACTIVE)||
00067         (motor->get_device_state(3) != INACTIVE)) {
00068       /* Bridge A was disabling due to overcurrent or over temperature */
00069       /* When at least on of its  motor was running */
00070         my_error_handler(0XBAD1);
00071     }
00072   }  
00073 }
00074 
00075 // CAN, to revise
00076 CAN can1(PB_8, PB_9);     // RX, TX
00077 
00078 CANMessage messageIn;
00079 CANMessage messageOut;
00080 
00081 void canrx()
00082 {
00083   while(1)
00084   {    
00085     if(can1.read(messageIn))
00086     {
00087       printf("READ!\n\r");
00088       
00089       if(messageIn.id == ((JOINT_SET_SPEED << 8) + JOINT_ID))
00090       {
00091           speed = 0;
00092           speed = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]);
00093           
00094           motor->set_speed(0, speed);
00095           (speed > 0) ? motor->run(0, BDCMotor::BWD) : motor->run(0, BDCMotor::FWD);
00096           
00097           printf("CAN: mess %d\n\r", speed);
00098       }
00099     }
00100     
00101     int speed = 5000;
00102     
00103     messageOut.data[0] = speed >> 24;
00104    messageOut.data[1] = speed >> 16;
00105    messageOut.data[2] = speed >> 8;
00106    messageOut.data[3] = speed;
00107    
00108    messageOut.id = (3 << 8) + 2;
00109     int status = can1.write(messageOut);
00110     
00111     printf("STATUS: %d\n\r", status);
00112     
00113     wait(0.01);
00114   }
00115 }
00116 
00117 
00118 /* Main ----------------------------------------------------------------------*/
00119 int main()
00120 {
00121   can1.frequency(125000);
00122   messageIn.format=CANExtended;
00123   messageOut.format=CANExtended;
00124   
00125   
00126   // Motor Initialization 
00127   
00128 #ifdef TARGET_STM32F429
00129     motor = new L6206(D2, A4, PB_4, PC_7, PA_15, PB_3);
00130 #else
00131     motor = new L6206(D2, A4, D5, D4, A0, A1);
00132 #endif
00133  
00134   if (motor->init(&init) != COMPONENT_OK) 
00135   {
00136     printf("ERROR: vvMotor Init\n\r");
00137     exit(EXIT_FAILURE);
00138   }
00139 
00140   motor->attach_flag_interrupt(my_flag_irq_handler);
00141   motor->attach_error_handler(my_error_handler);
00142 
00143   printf("DONE: Motor Init\n\r");
00144   
00145   // CAN Initialization
00146   
00147   canrxa.start(canrx);
00148   
00149   printf("DONE: CAN Init\n\r");
00150   
00151   //motor->set_speed(1, 50);
00152   //motor->run(1, BDCMotor::FWD);
00153   
00154   printf("Running!\n\r");
00155   
00156   while(true)
00157   {
00158     wait(1);
00159   }
00160 }