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