rac.cpp

00001 #include "mbed.h"
00002 #include "rac.h"
00003 #include "parameters.h"
00004 
00005 
00006 CANMessage rmsg;
00007 CAN can(p30, p29, 1000000);    // on definit pin et debit
00008 DigitalOut MOTOR_L_COM(LED1);
00009 DigitalOut MOTOR_R_COM(LED3);
00010 
00011 void RAC::readButtons(uint8_t SWX, uint8_t SWY, uint8_t BPO){ //define buttons pins
00012 
00013      _SWX = SWX;
00014      _SWY = SWY;
00015      _BPO = BPO;
00016     
00017     }
00018     
00019     
00020 
00021     void RAC::setVelocity(double velXin, double velYin, double centerRef, bool t_mode){
00022     ///////////// CONTROLE MOTEUR //////////////////////////////////////////////////////////////////////////////
00023     // LECTURE Joystick + stockage valeurs dans x, y
00024     _ctr_x = int(-((centerRef*100)-50));
00025     _ctr_y = int((centerRef*100)-50);
00026     if(_SWX){_x = int(-((velXin*100)-50))+_ctr_x;}else{_x=0;}    //  Detection x avec correction ctr de -42 à +42
00027     if(_SWY){_y = int((velYin*100)-50)+_ctr_y;}else{_y=0;}       //  Detection y avec correction ctr
00028     
00029     if(t_mode == 0)
00030     {
00031     ///////////// MODE DEGRADE /////////////////////////////////////
00032     _VX = (float(_x)/42)*ALPHA;    // Calcule brut vélocité x, y
00033     _VY = (float(_y)/42)*ALPHA; 
00034     _VM_L = _VX - _VY;            // Calcule des vélocités exactes de chaques moteurs
00035     _VM_R = -_VX - _VY;
00036         
00037     _command[0]= (_VM_L & 0x000000FF);        // Masque + décalage pour translation big vers little endian
00038     _command[1]= (_VM_L & 0x0000FF00)>>8;
00039     _command[2]= (_VM_L & 0x00FF0000)>>16;
00040     _command[3]= (_VM_L & 0xFF000000)>>24;
00041     send(RPDO1_L, _command, 'D', 4);         // Controle moteur gauche
00042         
00043     _command[0]= (_VM_R & 0x000000FF);
00044     _command[1]= (_VM_R & 0x0000FF00)>>8;
00045     _command[2]= (_VM_R & 0x00FF0000)>>16;
00046     _command[3]= (_VM_R & 0xFF000000)>>24;
00047     send(RPDO1_R, _command, 'D', 4);         // Controle moteur droit
00048     ////////////////////////////////////////////////////////////////
00049     }
00050     else if(t_mode == 1)
00051     {
00052     ///////////// MODE NORMAL //////////////////////////////////////
00053     _VX = _x;
00054     _VY = _y;
00055     //Left control
00056     _VM_L = ((_dx*_VX)-(_dy*_VY));
00057     _VM_L = (_VM_L/_l1);
00058     _VM_L = _VM_L*(ALPHA/42);
00059     //Right control
00060     _VM_R = ((-(_lx-_dx)*_VX)-(_dy*_VY));    
00061     _VM_R = (_VM_R/_l2); 
00062     _VM_R = _VM_R*(ALPHA/42);  
00063     
00064     _command[0]= (_VM_L & 0x000000FF);        // Masque + décalage pour translation big vers little endian
00065     _command[1]= (_VM_L & 0x0000FF00)>>8;
00066     _command[2]= (_VM_L & 0x00FF0000)>>16;
00067     _command[3]= (_VM_L & 0xFF000000)>>24;
00068     send(RPDO1_L, _command, 'D', 4);         // Controle moteur gauche
00069         
00070     _command[0]= (_VM_R & 0x000000FF);
00071     _command[1]= (_VM_R & 0x0000FF00)>>8;
00072     _command[2]= (_VM_R & 0x00FF0000)>>16;
00073     _command[3]= (_VM_R & 0xFF000000)>>24;
00074     send(RPDO1_R, _command, 'D', 4);         // Controle moteur droit
00075     }
00076     _rvx = _VM_R;
00077     _rvy = _VM_L;
00078     //////////////////////////////////////////////////////////////////////////////////////////////////////////////
00079     
00080     
00081     
00082     } 
00083     
00084     void RAC::readPosition(double *cX, double *cY, bool *t_mode){
00085     
00086     //Left motor      
00087         send(TPDO1_L, _command, 'R', 0); //Ask position for second length
00088         if(can.read(rmsg)){
00089         _p_actual = rmsg.data[3];
00090         _p_actual = _p_actual << 8;
00091         _p_actual = _p_actual | rmsg.data[2];
00092         _p_actual = _p_actual << 8;
00093         _p_actual = _p_actual | rmsg.data[1];
00094         _p_actual = _p_actual << 8;
00095         _p_actual = _p_actual | rmsg.data[0];
00096         MOTOR_L_COM = 1;
00097         }
00098         wait(0.045);
00099         //Right motor
00100         send(TPDO1_R, _command, 'R', 0); //Ask position for first length
00101         if(can.read(rmsg)){
00102         _p_actual2 = rmsg.data[3];
00103         _p_actual2 = _p_actual2 << 8;
00104         _p_actual2 = _p_actual2 | rmsg.data[2];
00105         _p_actual2 = _p_actual2 << 8;
00106         _p_actual2 = _p_actual2 | rmsg.data[1];
00107         _p_actual2 = _p_actual2 << 8;
00108         _p_actual2 = _p_actual2 | rmsg.data[0];
00109         MOTOR_R_COM = 1;
00110         }
00111         
00112         
00113         if(_BPO == 0){                      
00114          _p_pom = _p_actual; //Refresh first origin value
00115          _p_pom2 = _p_actual2; //Refresh second origin value
00116          *t_mode = !*t_mode; //Change mode 
00117          wait(1.0);
00118         }  
00119         *t_mode = *t_mode; //if nothing change
00120         
00121         //Calculate positions values in counters
00122         _p1 = _p_pom - _p_actual; 
00123         _p2 = _p_pom2 - _p_actual2;
00124         
00125         
00126         //Calculate Coordinates
00127         _l1 = LENGTH_L1_PO - (_p1/REFERENCE); //Length 1
00128         _l2 = LENGTH_L2_PO - (_p2/REFERENCE); //Length 2
00129         _lx = LENGTH_MOTOR;
00130         _dx = (((double) _l1*(double) _l1) - ((double) _l2*(double) _l2) + ((double) _lx*(double) _lx))/(2*(double) _lx);
00131         _dy = sqrt(((double) _l1*(double) _l1) - (_dx*_dx));
00132         *cX = _dx;
00133         *cY = _dy;
00134         MOTOR_L_COM = 0;
00135         MOTOR_R_COM = 0;
00136     
00137     
00138     } 
00139     
00140     
00141     void RAC::readVelocity(double *rvx, double *rvy)
00142     {
00143     //return velocities
00144     *rvx = _rvx;
00145     *rvy = _rvy;                
00146         
00147     }  
00148     
00149     
00150     
00151     void RAC::send(int id, char octet_emis[], char RouD, char longueur )
00152     {
00153         if (RouD == 'D')
00154         {  
00155         can.write(CANMessage(id, octet_emis, longueur, CANData, CANStandard )) ;  
00156         }
00157         else
00158         {  
00159         can.write(CANMessage(id, octet_emis, longueur,  CANRemote, CANStandard ));
00160         }
00161     }
00162