main.cpp

00001 #include "mbed.h"
00002 #include "L6206.h"
00003 
00004 #define MAX_MOTOR (2)
00005 
00006 static volatile uint16_t gLastError;
00007 static volatile uint8_t gStep = 0;
00008 
00009 int current_pose = 0;
00010 int speed = 0;
00011 
00012 L6206_init_t init =
00013 {
00014     L6206_CONF_PARAM_PARALLE_BRIDGES,
00015     {L6206_CONF_PARAM_FREQ_PWM1A, L6206_CONF_PARAM_FREQ_PWM2A, L6206_CONF_PARAM_FREQ_PWM1B, L6206_CONF_PARAM_FREQ_PWM2B},
00016     {100,100,100,100},
00017     {FORWARD,FORWARD,BACKWARD,FORWARD},
00018     {INACTIVE,INACTIVE,INACTIVE,INACTIVE},
00019     {FALSE,FALSE}
00020 };
00021 
00022 L6206 *motor;
00023 InterruptIn my_button_irq(USER_BUTTON); /* User button on Nucleo board */
00024 Thread canrxa;
00025 
00026 //Utility
00027 InterruptIn button(USER_BUTTON);
00028 DigitalOut led(LED1); //Change?
00029 
00030 
00031 void motor_error_handler(uint16_t error)
00032 {
00033   printf("ERROR: Motor Runtime\n\r");
00034   while(1){}; 
00035 }
00036 
00037 void motor_zero()
00038 {
00039   motor->run(0, BDCMotor::FWD);
00040   motor->run(1, BDCMotor::FWD);
00041 }
00042 
00043 void button_int_handler(unsigned int motorId)
00044 {
00045     printf("MOTOR SPEED: %d\n\r", motor->get_speed(motorId)); 
00046     motor_zero();
00047 }
00048 
00049 void my_error_handler(uint16_t error)
00050 {
00051   /* Backup error number */
00052   gLastError = error;
00053   
00054   /* Enter your own code here */
00055 }
00056 
00057 void my_flag_irq_handler(void)
00058 {
00059   /* Code to be customised */
00060   /************************/
00061   /* Get the state of bridge A */
00062   uint16_t bridgeState  = motor->get_bridge_status(0);
00063   
00064   if (bridgeState == 0) {
00065     if ((motor->get_device_state(0) != INACTIVE)||
00066         (motor->get_device_state(1) != INACTIVE)) {
00067       /* Bridge A was disabling due to overcurrent or over temperature */
00068       /* When at least on of its  motor was running */
00069         my_error_handler(0XBAD0);
00070     }
00071   }
00072   
00073   /* Get the state of bridge B */
00074   bridgeState  = motor->get_bridge_status(1);
00075   
00076   if (bridgeState == 0)  {
00077     if ((motor->get_device_state(2) != INACTIVE)||
00078         (motor->get_device_state(3) != INACTIVE)) {
00079       /* Bridge A was disabling due to overcurrent or over temperature */
00080       /* When at least on of its  motor was running */
00081         my_error_handler(0XBAD1);
00082     }
00083   }  
00084 }
00085 void end0_int_handler(unsigned int motorId)
00086 {
00087   printf("END0: Pressed\n\rSPEED: %d\n\r", motor->get_speed(motorId));
00088 }
00089 
00090 void end1_int_handler()
00091 {
00092   motor->hard_stop(0);
00093   motor->hard_stop(1); //or hard_hiz(); for disabling the bridge?
00094 
00095    motor->run(0, BDCMotor::BWD);
00096    motor->run(1, BDCMotor::BWD);
00097 
00098   printf("END1: Pressed\n\r");
00099 }
00100 
00101 
00102 
00103 // CAN, to revise
00104 CAN can1(PB_12, PB_13);     // RX, TX
00105 
00106 CANMessage messageIn;
00107 CANMessage messageOut;
00108 
00109 
00110 int filter = can1.filter(0x010, 0x4FF, CANStandard);
00111 
00112 void canrx()
00113 {
00114   while(1)
00115   {    
00116     if(can1.read(messageIn,filter)&& ((messageIn.id>>8 == 20) && (messageIn.id & 0x00FF==6))) //Primo motore
00117     {
00118       speed=messageIn.data[0]; //Messaggio da 0 a 255, devo sottrrarre 127 e imporre velocità con segno
00119       float speedMap=(speed-127)/127*100;
00120       printf("CAN: mess %f\n\r", speedMap);
00121       
00122       //CAN MESSAGE WITH SPEED TO REVISE
00123       //Ci sarebbe anche il set speed.
00124       if (speedMap == 0) 
00125       {
00126         motor->set_speed(0,0); //There's no soft stop. could it work like this?
00127         //current_speed= motor->get_speed(0); We could do lie this?
00128         //motor->go_to(current_pose); 
00129       }
00130       else if (speedMap>0)
00131         {
00132             motor->run(0,BDCMotor::FWD);
00133             motor->set_speed(0,(unsigned int) speedMap);
00134         }
00135       else if (speedMap<0)
00136       {
00137         motor->run(0,BDCMotor::BWD);
00138         motor->set_speed(0,(unsigned int) -speedMap);
00139       }
00140       else 
00141       {
00142         motor->set_speed(0,0); //Riportare errore? NO
00143       }
00144      }
00145      else if(can1.read(messageIn,filter)&& ((messageIn.id>>8 == 20) && (messageIn.id & 0x00FF==7))) //Secondo motore
00146     {
00147       speed=messageIn.data[0]; //Messaggio da 0 a 255, devo sottrrarre 127 e imporre velocità con segno
00148       float speedMap=(speed-127)/127*100;
00149       printf("CAN: mess %f\n\r", speedMap);
00150       
00151       //CAN MESSAGE WITH SPEED TO REVISE
00152       //Ci sarebbe anche il set speed.
00153       if (speedMap == 0) 
00154       {
00155         motor->set_speed(1,0); //There's no soft stop. could it work like this?
00156         //current_speed= motor->get_speed(0); We could do lie this?
00157         //motor->go_to(current_pose); 
00158       }
00159       else if (speedMap>0)
00160         {
00161             motor->run(1,BDCMotor::FWD);
00162             motor->set_speed(1,(unsigned int) speedMap);
00163         }
00164       else if (speedMap<0)
00165       {
00166         motor->run(1,BDCMotor::BWD);
00167         motor->set_speed(1,(unsigned int) -speedMap);
00168       }
00169       else 
00170       {
00171         motor->set_speed(0,0); //Riportare errore? NO
00172       }
00173      }
00174      }
00175 }
00176 //Aggiungere un get speed,FATTO  ci sarebbe anche il set_speed 
00177         //Dentro la libreria controllare se c'è controllo di Duty cycle FATTO
00178         //motor->go_to(current_pose);
00179 
00180 /* Main ----------------------------------------------------------------------*/
00181 
00182 int main()
00183 {
00184   can1.frequency(125000);
00185   messageIn.format=CANExtended;
00186   messageOut.format=CANExtended;
00187   // Motor Initialization 
00188   
00189 #ifdef TARGET_STM32F429
00190     motor = new L6206(D2, A4, PB_4, PC_7, PA_15, PB_3);
00191 #else
00192     motor = new L6206(D2, A4, D5, D4, A0, A1);
00193 #endif
00194  
00195   if (motor->init(&init) != COMPONENT_OK) 
00196   {
00197     printf("ERROR: vvMotor Init\n\r");
00198     exit(EXIT_FAILURE);
00199   }
00200 
00201   motor->attach_flag_interrupt(my_flag_irq_handler);
00202   motor->attach_error_handler(my_error_handler);
00203 
00204   
00205   printf("DONE: Motor Init\n\r");
00206   
00207   // CAN Initialization
00208   
00209   canrxa.start(canrx);
00210   
00211   printf("DONE: CAN Init\n\r");
00212   
00213 
00214   
00215   printf("Running!\n\r");
00216   
00217   while(true)
00218   {
00219     wait(1000);
00220   }
00221 }