main.cpp

00001 #include "mbed.h"
00002 #include "L6208.h"
00003 
00004 #define VREFA_PWM_PIN D3
00005 #define VREFB_PWM_PIN D9
00006 
00007 #define JOINT_SET_SPEED 20
00008 
00009 #define JOINT_ID 2
00010 
00011 l6208_init_t init =
00012 {
00013   8000,            //Acceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
00014   80,              //Acceleration current torque in % (from 0 to 100)
00015   8000,            //Deceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
00016   80,              //Deceleration current torque in % (from 0 to 100)
00017   8000,            //Running speed in step/s or (1/16)th step/s for microstep modes
00018   80,              //Running current torque in % (from 0 to 100)
00019   40,              //Holding current torque in % (from 0 to 100)
00020   STEP_MODE_1_16,  //Step mode via enum motorStepMode_t
00021   FAST_DECAY,      //Decay mode via enum motorDecayMode_t
00022   0,               //Dwelling time in ms
00023   FALSE,           //Automatic HIZ STOP
00024   100000           //VREFA and VREFB PWM frequency (Hz)
00025 };
00026 
00027 Thread canrxa;
00028 
00029 // Utility
00030 InterruptIn button(USER_BUTTON);
00031 DigitalOut led(LED1);
00032 
00033 // Motor Control
00034 L6208 *motor;
00035 
00036 InterruptIn end0(PC_4, PullUp);
00037 InterruptIn end1(PC_8, PullUp);
00038 InterruptIn enc(PC_12, PullUp);
00039 
00040 int32_t speed = 0;
00041 
00042 void motor_error_handler(uint16_t error)
00043 {
00044     printf("ERROR: Motor Runtime\n\r");
00045     while(1){} 
00046 }
00047 
00048 void motor_zero()
00049 {
00050     motor->run(StepperMotor::BWD);
00051 }
00052 
00053 void button_int_handler()
00054 {
00055     printf("POSITION: %d\n\r", motor->get_position());
00056     motor_zero();
00057 }
00058 
00059 void end0_pressed()
00060 {
00061     motor->run(StepperMotor::BWD);
00062     printf("END0: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
00063 }
00064 
00065 void end0_released()
00066 {
00067     motor->go_to(motor->get_position());
00068 }
00069 
00070 void end1_pressed()
00071 {
00072     motor->hard_stop();
00073     motor->run(StepperMotor::FWD);
00074 
00075     printf("END1: Pressed\n\r");
00076 }
00077 
00078 void motor_set_home()
00079 {
00080   motor->hard_stop();
00081   motor->set_home();
00082   motor->go_to(0);
00083 }
00084 
00085 // CAN
00086 CAN can1(PB_8, PB_9);     // RX, TX
00087 
00088 CANMessage messageIn;
00089 CANMessage messageOut;
00090 
00091 void canrx()
00092 {
00093   while(1)
00094   {    
00095     if(can1.read(messageIn))
00096     {
00097       printf("CAN: message received\r\n");
00098       if(messageIn.id == ((JOINT_SET_SPEED << 8) + JOINT_ID))
00099       {
00100           speed = 0;
00101           speed = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]);
00102           
00103           motor->set_max_speed(speed);
00104           (speed > 0) ? motor->run(StepperMotor::BWD) : motor->run(StepperMotor::FWD);
00105           
00106           printf("CAN: setting speed -> %d\n\r", speed);
00107       }
00108     }
00109     
00110     wait(0.05);
00111   }
00112 }
00113 
00114 
00115 /* Main ----------------------------------------------------------------------*/
00116 
00117 int main()
00118 {
00119   can1.frequency(125000);
00120   messageIn.format=CANExtended;
00121   
00122   // Motor Initialization 
00123   motor = new L6208(D2, D8, D7, D4, D5, D6, VREFA_PWM_PIN, VREFB_PWM_PIN);
00124   
00125   if (motor->init(&init) != COMPONENT_OK)
00126   {
00127     printf("ERROR: vvMotor Init\n\r");
00128     exit(EXIT_FAILURE);
00129   }
00130 
00131   motor->attach_error_handler(&motor_error_handler);
00132   
00133   // Limit EndStop
00134   end0.rise(&end0_pressed);
00135   end0.fall(&end0_released);
00136   // Zero EndStop
00137   end1.rise(&end1_pressed);
00138   end1.fall(&motor_set_home);
00139   
00140   button.rise(&button_int_handler);
00141   
00142   motor->set_step_mode(StepperMotor::STEP_MODE_1_8);
00143   
00144   motor_set_home();
00145   
00146   printf("DONE: Motor Init\n\r");
00147   
00148   // CAN Initialization
00149   
00150   canrxa.start(canrx);
00151   
00152   printf("DONE: CAN Init\n\r");
00153   
00154   printf("Running!\n\r");
00155   
00156   // DEBUG
00157   //motor->set_max_speed(8000);
00158   //motor->run(StepperMotor::FWD);
00159   
00160   while(true)
00161   {
00162     wait(1);
00163   }
00164 }