actiondrv.cpp

00001 #include "mbed.h"
00002 #include "actiondrv.h"
00003 
00004 static char msgsetmode[] = {0x2B,0x60,0x60,0x03,0x00,0x00,0x00,0x00};
00005 static char msgsetacc[] = {0x23,0x83,0x60,0x00,0x80,0x96,0x98,0x00};
00006 static char msgsetdec[] = {0x23,0x84,0x60,0x00,0x80,0x96,0x98,0x00};
00007 static char msgsetvel[] = {0x23,0xff,0x60,0x00,0x00,0x00,0x00,0x00};
00008 //static char heartbeat[] = {0x2B,0x17,0x10,0x00,0x64,0x00,0x00,0x00};
00009 static char msgenable[] = {0x01,0x00};
00010 static char enablemotor[] = {0x2B,0x40,0x60,0x00,0x0F,0x00,0x00,0x00};
00011 DigitalOut led1(LED1);
00012 CAN can1(PA_11, PA_12, 500000);
00013 
00014 int rpm_to_pulse_per_s = 500*4/60;
00015 
00016 actionDrv::actionDrv(int _id)
00017 {
00018     id = _id;
00019        
00020 }
00021 
00022 void actionDrv::send(char* msg)
00023 {
00024     //if(can1.write(CANMessage(0x600+ id, msg)))
00025     //{
00026     // led1 = !led1;
00027     // wait(0.05);    
00028     //}   
00029     
00030     while(!can1.write(CANMessage(0x600+ id, msg))) {wait(0.05);}
00031 }
00032 
00033 
00034 void actionDrv::Enable(){
00035     //msgenable[1] = ((id >>24)& 0xFF);
00036     //can1.write(CANMessage(0x000, msgenable));
00037     //wait(0.2);
00038     //send(enablemotor);
00039     
00040     msgenable[1] = ((id >>24)& 0xFF);
00041     while(!can1.write(CANMessage(0x000, msgenable)));
00042     wait(0.2);
00043     send(enablemotor);
00044     }
00045 void actionDrv::SetOperationalMode(){
00046     send(msgsetmode);
00047     }
00048 void actionDrv::Configvelocity(int acc, int dec){
00049     
00050     msgsetacc[4] = ((acc ) & 0xFF);
00051     msgsetacc[5] = ((acc >> 8) & 0xFF);
00052     msgsetacc[6] = ((acc >> 16) & 0xFF);
00053     msgsetacc[7] = ((acc >> 24) & 0xFF);
00054     msgsetdec[4] = ((dec ) & 0xFF);
00055     msgsetdec[5] = ((dec >> 8) & 0xFF);
00056     msgsetdec[6] = ((dec >> 16) & 0xFF);
00057     msgsetdec[7] = ((dec >> 24)  & 0xFF);
00058     
00059     send(msgsetacc);
00060     send(msgsetdec);
00061     }
00062 void actionDrv::SetVelocity(int vel){
00063     //vel = vel*500*4/60; // rpm to pulse per s
00064     vel = vel*rpm_to_pulse_per_s;
00065     msgsetvel[4] = ((vel ) &0xFF);
00066     msgsetvel[5] = ((vel >> 8) &0xFF);
00067     msgsetvel[6] = ((vel >> 16) &0xFF);
00068     msgsetvel[7] = ((vel >> 24) & 0xFF);
00069     send(msgsetvel);
00070     }
00071     
00072 void actionDrv::send_mod(char* msg)
00073 {
00074     //if(can1.write(CANMessage(0x600+ id, msg)))
00075     //{
00076     // led1 = !led1;    
00077     //}   
00078     
00079     while(!can1.write(CANMessage(0x600+ id, msg)));
00080 }
00081  
00082 void actionDrv::SetVelocity_mod(int vel){
00083     //vel = vel*500*4/60; // rpm to pulse per s
00084     vel = vel*rpm_to_pulse_per_s;
00085     msgsetvel[4] = ((vel ) &0xFF);
00086     msgsetvel[5] = ((vel >> 8) &0xFF);
00087     msgsetvel[6] = ((vel >> 16) &0xFF);
00088     msgsetvel[7] = ((vel >> 24) & 0xFF);
00089     send_mod(msgsetvel);
00090     }
00091     
00092 
00093     
00094 void actionDrv::stop(){
00095     SetVelocity(0);
00096     
00097     }
00098 
00099