File content as of revision 12:98a051edb19a:

#include "mbed.h"
#include "rac.h"
#include "parameters.h"


CANMessage rmsg;
CAN can(p30, p29, 1000000);    // on definit pin et debit
DigitalOut MOTOR_L_COM(LED1);
DigitalOut MOTOR_R_COM(LED3);

void RAC::readButtons(uint8_t SWX, uint8_t SWY, uint8_t BPO){ //define buttons pins

     _SWX = SWX;
     _SWY = SWY;
     _BPO = BPO;
    
    }
    
    

    void RAC::setVelocity(double velXin, double velYin, double centerRef, bool t_mode){
    ///////////// CONTROLE MOTEUR //////////////////////////////////////////////////////////////////////////////
    // LECTURE Joystick + stockage valeurs dans x, y
    _ctr_x = int(-((centerRef*100)-50));
    _ctr_y = int((centerRef*100)-50);
    if(_SWX){_x = int(-((velXin*100)-50))+_ctr_x;}else{_x=0;}    //  Detection x avec correction ctr de -42 à +42
    if(_SWY){_y = int((velYin*100)-50)+_ctr_y;}else{_y=0;}       //  Detection y avec correction ctr
    
    if(t_mode == 0)
    {
    ///////////// MODE DEGRADE /////////////////////////////////////
    _VX = (float(_x)/42)*ALPHA;    // Calcule brut vélocité x, y
    _VY = (float(_y)/42)*ALPHA; 
    _VM_L = _VX - _VY;            // Calcule des vélocités exactes de chaques moteurs
    _VM_R = -_VX - _VY;
        
    _command[0]= (_VM_L & 0x000000FF);        // Masque + décalage pour translation big vers little endian
    _command[1]= (_VM_L & 0x0000FF00)>>8;
    _command[2]= (_VM_L & 0x00FF0000)>>16;
    _command[3]= (_VM_L & 0xFF000000)>>24;
    send(RPDO1_L, _command, 'D', 4);         // Controle moteur gauche
        
    _command[0]= (_VM_R & 0x000000FF);
    _command[1]= (_VM_R & 0x0000FF00)>>8;
    _command[2]= (_VM_R & 0x00FF0000)>>16;
    _command[3]= (_VM_R & 0xFF000000)>>24;
    send(RPDO1_R, _command, 'D', 4);         // Controle moteur droit
    ////////////////////////////////////////////////////////////////
    }
    else if(t_mode == 1)
    {
    ///////////// MODE NORMAL //////////////////////////////////////
    _VX = _x;
    _VY = _y;
    //Left control
    _VM_L = ((_dx*_VX)-(_dy*_VY));
    _VM_L = (_VM_L/_l1);
    _VM_L = _VM_L*(ALPHA/42);
    //Right control
    _VM_R = ((-(_lx-_dx)*_VX)-(_dy*_VY));    
    _VM_R = (_VM_R/_l2); 
    _VM_R = _VM_R*(ALPHA/42);  
    
    _command[0]= (_VM_L & 0x000000FF);        // Masque + décalage pour translation big vers little endian
    _command[1]= (_VM_L & 0x0000FF00)>>8;
    _command[2]= (_VM_L & 0x00FF0000)>>16;
    _command[3]= (_VM_L & 0xFF000000)>>24;
    send(RPDO1_L, _command, 'D', 4);         // Controle moteur gauche
        
    _command[0]= (_VM_R & 0x000000FF);
    _command[1]= (_VM_R & 0x0000FF00)>>8;
    _command[2]= (_VM_R & 0x00FF0000)>>16;
    _command[3]= (_VM_R & 0xFF000000)>>24;
    send(RPDO1_R, _command, 'D', 4);         // Controle moteur droit
    }
    _rvx = _VM_R;
    _rvy = _VM_L;
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////
    
    
    
    } 
    
    void RAC::readPosition(double *cX, double *cY, bool *t_mode){
    
    //Left motor      
        send(TPDO1_L, _command, 'R', 0); //Ask position for second length
        if(can.read(rmsg)){
        _p_actual = rmsg.data[3];
        _p_actual = _p_actual << 8;
        _p_actual = _p_actual | rmsg.data[2];
        _p_actual = _p_actual << 8;
        _p_actual = _p_actual | rmsg.data[1];
        _p_actual = _p_actual << 8;
        _p_actual = _p_actual | rmsg.data[0];
        MOTOR_L_COM = 1;
        }
        wait(0.045);
        //Right motor
        send(TPDO1_R, _command, 'R', 0); //Ask position for first length
        if(can.read(rmsg)){
        _p_actual2 = rmsg.data[3];
        _p_actual2 = _p_actual2 << 8;
        _p_actual2 = _p_actual2 | rmsg.data[2];
        _p_actual2 = _p_actual2 << 8;
        _p_actual2 = _p_actual2 | rmsg.data[1];
        _p_actual2 = _p_actual2 << 8;
        _p_actual2 = _p_actual2 | rmsg.data[0];
        MOTOR_R_COM = 1;
        }
        
        
        if(_BPO == 0){                      
         _p_pom = _p_actual; //Refresh first origin value
         _p_pom2 = _p_actual2; //Refresh second origin value
         *t_mode = !*t_mode; //Change mode 
         wait(1.0);
        }  
        *t_mode = *t_mode; //if nothing change
        
        //Calculate positions values in counters
        _p1 = _p_pom - _p_actual; 
        _p2 = _p_pom2 - _p_actual2;
        
        
        //Calculate Coordinates
        _l1 = LENGTH_L1_PO - (_p1/REFERENCE); //Length 1
        _l2 = LENGTH_L2_PO - (_p2/REFERENCE); //Length 2
        _lx = LENGTH_MOTOR;
        _dx = (((double) _l1*(double) _l1) - ((double) _l2*(double) _l2) + ((double) _lx*(double) _lx))/(2*(double) _lx);
        _dy = sqrt(((double) _l1*(double) _l1) - (_dx*_dx));
        *cX = _dx;
        *cY = _dy;
        MOTOR_L_COM = 0;
        MOTOR_R_COM = 0;
    
    
    } 
    
    
    void RAC::readVelocity(double *rvx, double *rvy)
    {
    //return velocities
    *rvx = _rvx;
    *rvy = _rvy;                
        
    }  
    
    
    
    void RAC::send(int id, char octet_emis[], char RouD, char longueur )
    {
        if (RouD == 'D')
        {  
        can.write(CANMessage(id, octet_emis, longueur, CANData, CANStandard )) ;  
        }
        else
        {  
        can.write(CANMessage(id, octet_emis, longueur,  CANRemote, CANStandard ));
        }
    }