main.cpp

00001 #include "mbed.h"
00002 #include "L6208.h"
00003 
00004 #define VREFA_PWM_PIN D3
00005 #define VREFB_PWM_PIN D9
00006 #define BAUDRATE 9600
00007 #define JOINT 2
00008 l6208_init_t init =
00009 {
00010   8000,            //Acceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
00011   80,              //Acceleration current torque in % (from 0 to 100)
00012   8000,            //Deceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
00013   80,              //Deceleration current torque in % (from 0 to 100)
00014   8000,            //Running speed in step/s or (1/16)th step/s for microstep modes
00015   80,              //Running current torque in % (from 0 to 100)
00016   40,              //Holding current torque in % (from 0 to 100)
00017   STEP_MODE_1_16,  //Step mode via enum motorStepMode_t
00018   FAST_DECAY,      //Decay mode via enum motorDecayMode_t
00019   0,               //Dwelling time in ms
00020   FALSE,           //Automatic HIZ STOP
00021   100000           //VREFA and VREFB PWM frequency (Hz)
00022 };
00023 
00024 
00025 // Utility
00026 //InterruptIn button(USER_BUTTON);
00027 DigitalOut led(LED1);
00028 
00029 // Motor Control
00030 L6208 *motor;
00031 
00032 InterruptIn end1(USER_BUTTON, PullUp);
00033 DigitalIn end0(PA_5, PullUp);
00034 Serial serial(PA_2, PA_3); 
00035 
00036 
00037 
00038 float pose, current_pose;
00039 float speed, current_speed;
00040 void zero()
00041 {
00042   printf("zero");
00043   motor->run(StepperMotor::BWD);
00044   while(!end0){
00045   }
00046   motor->hard_stop();
00047   motor->set_home();
00048   motor->go_to(0);
00049   printf("END0: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
00050 }
00051 
00052 void motor_error_handler(uint16_t error)
00053 {
00054   printf("ERROR: Motor Runtime\n\r");
00055   
00056 }
00057 
00058 void end1_int_handler()
00059 {
00060   // motor->hard_stop();
00061   
00062   motor->run(StepperMotor::FWD);
00063   
00064   printf("END1: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
00065 }
00066 
00067 
00068 void serialrx()
00069 {
00070 
00071   int id, speed;
00072   
00073   
00074     if(serial.readable() ) 
00075     {
00076       serial.scanf("%d %d", &id, &speed );
00077       printf("%d %d\n", id, speed);
00078       if (id==JOINT)
00079       {
00080         led=!led;
00081         current_speed=speed;
00082       }
00083     
00084     
00085   }
00086 }
00087   
00088   void fmotor()
00089   {
00090   
00091       
00092       if (current_speed>0)
00093       {
00094         printf("run FWD\n");
00095         motor->set_max_speed(abs(current_speed*80));
00096         motor->run(StepperMotor::BWD);
00097       }
00098       else if (current_speed<0)
00099       {
00100         printf("run BWD\n");
00101         motor->set_max_speed(abs(current_speed*80));
00102         motor->run(StepperMotor::FWD);
00103       }
00104       
00105       else
00106       {
00107         motor->hard_stop();
00108         current_pose= motor->get_position();
00109         motor->go_to(current_pose);
00110       }
00111       
00112       
00113       
00114     
00115   }
00116   
00117   
00118   /* Main ----------------------------------------------------------------------*/
00119   
00120   int main()
00121   {
00122     led=1;
00123     serial.baud(BAUDRATE);
00124     
00125     // Motor Initialization
00126     motor = new L6208(D2, D8, D7, D4, D5, D6, VREFA_PWM_PIN, VREFB_PWM_PIN);
00127     motor->set_step_mode(StepperMotor::STEP_MODE_1_16);
00128     if (motor->init(&init) != COMPONENT_OK)
00129     {
00130       printf("ERROR: vvMotor Init\n\r");
00131       exit(EXIT_FAILURE);
00132     }
00133     
00134     motor->attach_error_handler(&motor_error_handler);
00135     
00136     
00137    // end1.rise(&end1_int_handler);
00138     
00139     printf("DONE: Motor Init\n\r");
00140     
00141     
00142     printf("Running!\n\r");
00143     
00144     //zero();
00145     
00146     while(true)
00147     {
00148       serialrx();
00149       //wait (0.001);
00150       fmotor();
00151       
00152     }
00153   }