added serial up, down, left and right control
Fork of Robot_a_cables_v3_5 by
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 )); + } + } +