![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Include new libraries normal mode updated(offset reduced)
Dependencies: mbed
Fork of Robot_a_cables_v2_6_5 by
rac.cpp
- Committer:
- protongamer
- Date:
- 2018-05-28
- Revision:
- 10:fb3542f3c5e6
File content as of revision 10:fb3542f3c5e6:
#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 )); } }