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