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 SEND_FREQUENCY 10 //Hz
00007 
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 // CAN
00025 CAN can1(PA_11, PA_12);     // RX, TX
00026 
00027 CANMessage messageIn;
00028 CANMessage messageOut;
00029 
00030 Thread cantxa(osPriorityNormal);
00031 Thread canrxa(osPriorityNormal);
00032 
00033 // Utility
00034 //InterruptIn button(USER_BUTTON);
00035 DigitalOut led(LED1);
00036 
00037 // Motor Control
00038 L6208 *motor;
00039 
00040 InterruptIn end0(PA_5, PullUp);
00041 InterruptIn end1(PA_6, PullUp);
00042 //InterruptIn enc(PC_12, PullUp);
00043 
00044 typedef enum
00045 {
00046     JOINT_SET_SPEED = 20,
00047     JOINT_SET_POSITION,
00048     JOINT_CURRENT_POSITION,
00049     JOINT_CURRENT_SPEED,
00050     JOINT_STATUS,
00051     JOINT_ERROR,
00052     JOINT_TORQUE,
00053     JOINT_MAXTORQUE,
00054     JOINT_ZERO,
00055 }CAN_COMMANDS;
00056 
00057 typedef enum
00058 {
00059     BASE=1,
00060     SHOULDER,
00061     ELBOW,
00062     WRIST1,
00063     WRIST2,
00064     WRIST3,
00065     END_EFFECTOR,
00066     CAMERA1,
00067     CAMERA2,
00068 }JOINT;
00069 
00070 float pose, current_pose;
00071 float speed, current_speed;
00072 
00073 void set_zero()
00074 {
00075     printf("set zero\t\n");
00076     
00077     motor->hard_stop();
00078     motor->set_home();
00079     motor->go_to(0);
00080 }
00081 
00082 void zero()
00083 {
00084     printf("zero");
00085     motor->run(StepperMotor::FWD);
00086 }
00087 
00088 uint32_t gen_can_id(CAN_COMMANDS message_id, JOINT can_id)
00089 {
00090     uint32_t id = (uint32_t)can_id;     // LSB byte is the controller id.
00091     id |= (uint32_t)message_id << 8;  // Next lowest byte is the packet id.
00092     id |= 0x80000000;              // Send in Extended Frame Format.
00093     return id;
00094 }
00095 
00096 double to_rad(double angle)
00097 {
00098     return angle*0.0174533;
00099 }
00100 
00101 double angle_deparse (long int pose, float offset)
00102 {
00103     offset = offset * 0.00872664625;
00104     double angle = pose *0.000487012987; //do something 0,0004791666667
00105     angle = (angle - offset);
00106     return angle;
00107 }
00108 
00109 void motor_error_handler(uint16_t error)
00110 {
00111     printf("ERROR: Motor Runtime\n\r");
00112 }
00113 
00114 void end0_int_handler()
00115 {
00116     motor->hard_stop();
00117     motor->run(StepperMotor::BWD);
00118     
00119     printf("END0: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
00120 }
00121 
00122 void end0_int_handler_fall()
00123 {
00124     motor->hard_stop();
00125     int position = motor->get_position();
00126     motor->go_to(position);
00127 }
00128 
00129 void end1_int_handler()
00130 {
00131     motor->hard_stop();
00132     motor->run(StepperMotor::FWD);
00133     
00134     printf("END1: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
00135 }
00136 
00137 void cantx ()
00138 {
00139     while(1)
00140     {
00141         int _pose;
00142         
00143         messageOut.format = CANExtended;
00144         messageOut.id=gen_can_id(JOINT_CURRENT_POSITION, SHOULDER);
00145         
00146         pose=angle_deparse(motor->get_position(), 0);
00147         _pose=pose*100;
00148         
00149         messageOut.data[3]=_pose;
00150         messageOut.data[2]=_pose >>8;
00151         messageOut.data[1]=_pose >>16;
00152         messageOut.data[0]=_pose >>24;
00153         
00154         int status = can1.write(messageOut);
00155         printf("CAN send CURRENT POSITION Joint status %d : pose %f\t\n",status, pose);
00156     }
00157 }
00158 
00159 void cantx_ISR()
00160 {
00161     cantx();
00162     osDelay(1/SEND_FREQUENCY*1000);
00163 }
00164 
00165 void canrx()
00166 {
00167     while(1)
00168     {   
00169         if(can1.read(messageIn))
00170         {
00171             led = !led;
00172             
00173             printf("Read!\n\r");
00174             
00175             if(messageIn.id==gen_can_id(JOINT_SET_SPEED,SHOULDER))
00176             {
00177                 speed=messageIn.data[3] + (messageIn.data[2] << 8) + (messageIn.data[1] << 16) + (messageIn.data[0] << 24);
00178                 printf("CAN: mess  received  : %f\n\r", speed);
00179                 current_speed=speed;
00180                 
00181                 if (current_speed>0)
00182                 {
00183                     motor->set_max_speed(current_speed*80);
00184                     motor->run(StepperMotor::FWD);
00185                 }
00186                 else if (current_speed<0)
00187                 {
00188                     motor->set_max_speed(current_speed*80);
00189                     motor->run(StepperMotor::BWD);
00190                 }
00191                 else
00192                 {
00193                     motor->soft_stop();
00194                     current_pose= motor->get_position();
00195                     motor->go_to(current_pose);
00196                 }
00197             }
00198             else if(messageIn.id==gen_can_id(JOINT_ZERO,SHOULDER))
00199             {
00200                 if((messageIn.data[3] + (messageIn.data[2] << 8) + (messageIn.data[1] << 16) + (messageIn.data[0] << 24))==1)
00201                 {
00202                     zero();
00203                     motor->wait_while_active();
00204                 }
00205             }
00206         }
00207     }
00208 }
00209 
00210 void canrx_ISR()
00211 {
00212     canrx();
00213     osDelay(10);
00214 }
00215 
00216 /* Main ----------------------------------------------------------------------*/
00217 
00218 int main()
00219 {
00220     
00221     can1.frequency(125000);
00222     
00223     // Motor Initialization
00224     motor = new L6208(D2, D8, D7, D4, D5, D6, VREFA_PWM_PIN, VREFB_PWM_PIN);
00225     motor->set_step_mode(StepperMotor::STEP_MODE_1_16);
00226     
00227     if (motor->init(&init) != COMPONENT_OK)
00228     {
00229         printf("ERROR: vvMotor Init\n\r");
00230         exit(EXIT_FAILURE);
00231     }
00232     
00233     motor->attach_error_handler(&motor_error_handler);
00234     
00235     end0.rise(&end0_int_handler);
00236     end0.fall(&end0_int_handler_fall);
00237     end1.rise(&end1_int_handler);
00238     end1.fall(&set_zero);
00239     
00240     printf("DONE: Motor Init\n\r");
00241     
00242     // CAN Initialization
00243     // zero();
00244     
00245     canrxa.start(canrx_ISR);
00246     //cantxa.start(cantx_ISR);
00247     
00248     printf("DONE: CAN Init\n\r");
00249     
00250     printf("Running!\n\r");
00251     
00252     //zero();
00253     
00254     while(true)
00255     {
00256         wait(1000);
00257     }
00258 }