Diff: rac.cpp

Revision:

10:fb3542f3c5e6

diff -r 0f63d4cb5613 -r fb3542f3c5e6 rac.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/rac.cpp	Mon May 28 14:18:32 2018 +0000
@@ -0,0 +1,162 @@
+#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 ));
+        }
+    }
+