added serial up, down, left and right control
Dependencies: mbed
Fork of Robot_a_cables_v3_6 by
rac.cpp@10:fb3542f3c5e6, 2018-05-28 (annotated)
- Committer:
- protongamer
- Date:
- Mon May 28 14:18:32 2018 +0000
- Revision:
- 10:fb3542f3c5e6
Version 3.5;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
protongamer | 10:fb3542f3c5e6 | 1 | #include "mbed.h" |
protongamer | 10:fb3542f3c5e6 | 2 | #include "rac.h" |
protongamer | 10:fb3542f3c5e6 | 3 | #include "parameters.h" |
protongamer | 10:fb3542f3c5e6 | 4 | |
protongamer | 10:fb3542f3c5e6 | 5 | |
protongamer | 10:fb3542f3c5e6 | 6 | CANMessage rmsg; |
protongamer | 10:fb3542f3c5e6 | 7 | CAN can(p30, p29, 1000000); // on definit pin et debit |
protongamer | 10:fb3542f3c5e6 | 8 | DigitalOut MOTOR_L_COM(LED1); |
protongamer | 10:fb3542f3c5e6 | 9 | DigitalOut MOTOR_R_COM(LED3); |
protongamer | 10:fb3542f3c5e6 | 10 | |
protongamer | 10:fb3542f3c5e6 | 11 | void RAC::readButtons(uint8_t SWX, uint8_t SWY, uint8_t BPO){ //define buttons pins |
protongamer | 10:fb3542f3c5e6 | 12 | |
protongamer | 10:fb3542f3c5e6 | 13 | _SWX = SWX; |
protongamer | 10:fb3542f3c5e6 | 14 | _SWY = SWY; |
protongamer | 10:fb3542f3c5e6 | 15 | _BPO = BPO; |
protongamer | 10:fb3542f3c5e6 | 16 | |
protongamer | 10:fb3542f3c5e6 | 17 | } |
protongamer | 10:fb3542f3c5e6 | 18 | |
protongamer | 10:fb3542f3c5e6 | 19 | |
protongamer | 10:fb3542f3c5e6 | 20 | |
protongamer | 10:fb3542f3c5e6 | 21 | void RAC::setVelocity(double velXin, double velYin, double centerRef, bool t_mode){ |
protongamer | 10:fb3542f3c5e6 | 22 | ///////////// CONTROLE MOTEUR ////////////////////////////////////////////////////////////////////////////// |
protongamer | 10:fb3542f3c5e6 | 23 | // LECTURE Joystick + stockage valeurs dans x, y |
protongamer | 10:fb3542f3c5e6 | 24 | _ctr_x = int(-((centerRef*100)-50)); |
protongamer | 10:fb3542f3c5e6 | 25 | _ctr_y = int((centerRef*100)-50); |
protongamer | 10:fb3542f3c5e6 | 26 | if(_SWX){_x = int(-((velXin*100)-50))+_ctr_x;}else{_x=0;} // Detection x avec correction ctr de -42 à +42 |
protongamer | 10:fb3542f3c5e6 | 27 | if(_SWY){_y = int((velYin*100)-50)+_ctr_y;}else{_y=0;} // Detection y avec correction ctr |
protongamer | 10:fb3542f3c5e6 | 28 | |
protongamer | 10:fb3542f3c5e6 | 29 | if(t_mode == 0) |
protongamer | 10:fb3542f3c5e6 | 30 | { |
protongamer | 10:fb3542f3c5e6 | 31 | ///////////// MODE DEGRADE ///////////////////////////////////// |
protongamer | 10:fb3542f3c5e6 | 32 | _VX = (float(_x)/42)*ALPHA; // Calcule brut vélocité x, y |
protongamer | 10:fb3542f3c5e6 | 33 | _VY = (float(_y)/42)*ALPHA; |
protongamer | 10:fb3542f3c5e6 | 34 | _VM_L = _VX - _VY; // Calcule des vélocités exactes de chaques moteurs |
protongamer | 10:fb3542f3c5e6 | 35 | _VM_R = -_VX - _VY; |
protongamer | 10:fb3542f3c5e6 | 36 | |
protongamer | 10:fb3542f3c5e6 | 37 | _command[0]= (_VM_L & 0x000000FF); // Masque + décalage pour translation big vers little endian |
protongamer | 10:fb3542f3c5e6 | 38 | _command[1]= (_VM_L & 0x0000FF00)>>8; |
protongamer | 10:fb3542f3c5e6 | 39 | _command[2]= (_VM_L & 0x00FF0000)>>16; |
protongamer | 10:fb3542f3c5e6 | 40 | _command[3]= (_VM_L & 0xFF000000)>>24; |
protongamer | 10:fb3542f3c5e6 | 41 | send(RPDO1_L, _command, 'D', 4); // Controle moteur gauche |
protongamer | 10:fb3542f3c5e6 | 42 | |
protongamer | 10:fb3542f3c5e6 | 43 | _command[0]= (_VM_R & 0x000000FF); |
protongamer | 10:fb3542f3c5e6 | 44 | _command[1]= (_VM_R & 0x0000FF00)>>8; |
protongamer | 10:fb3542f3c5e6 | 45 | _command[2]= (_VM_R & 0x00FF0000)>>16; |
protongamer | 10:fb3542f3c5e6 | 46 | _command[3]= (_VM_R & 0xFF000000)>>24; |
protongamer | 10:fb3542f3c5e6 | 47 | send(RPDO1_R, _command, 'D', 4); // Controle moteur droit |
protongamer | 10:fb3542f3c5e6 | 48 | //////////////////////////////////////////////////////////////// |
protongamer | 10:fb3542f3c5e6 | 49 | } |
protongamer | 10:fb3542f3c5e6 | 50 | else if(t_mode == 1) |
protongamer | 10:fb3542f3c5e6 | 51 | { |
protongamer | 10:fb3542f3c5e6 | 52 | ///////////// MODE NORMAL ////////////////////////////////////// |
protongamer | 10:fb3542f3c5e6 | 53 | _VX = _x; |
protongamer | 10:fb3542f3c5e6 | 54 | _VY = _y; |
protongamer | 10:fb3542f3c5e6 | 55 | //Left control |
protongamer | 10:fb3542f3c5e6 | 56 | _VM_L = ((_dx*_VX)-(_dy*_VY)); |
protongamer | 10:fb3542f3c5e6 | 57 | _VM_L = (_VM_L/_l1); |
protongamer | 10:fb3542f3c5e6 | 58 | _VM_L = _VM_L*(ALPHA/42); |
protongamer | 10:fb3542f3c5e6 | 59 | //Right control |
protongamer | 10:fb3542f3c5e6 | 60 | _VM_R = ((-(_lx-_dx)*_VX)-(_dy*_VY)); |
protongamer | 10:fb3542f3c5e6 | 61 | _VM_R = (_VM_R/_l2); |
protongamer | 10:fb3542f3c5e6 | 62 | _VM_R = _VM_R*(ALPHA/42); |
protongamer | 10:fb3542f3c5e6 | 63 | |
protongamer | 10:fb3542f3c5e6 | 64 | _command[0]= (_VM_L & 0x000000FF); // Masque + décalage pour translation big vers little endian |
protongamer | 10:fb3542f3c5e6 | 65 | _command[1]= (_VM_L & 0x0000FF00)>>8; |
protongamer | 10:fb3542f3c5e6 | 66 | _command[2]= (_VM_L & 0x00FF0000)>>16; |
protongamer | 10:fb3542f3c5e6 | 67 | _command[3]= (_VM_L & 0xFF000000)>>24; |
protongamer | 10:fb3542f3c5e6 | 68 | send(RPDO1_L, _command, 'D', 4); // Controle moteur gauche |
protongamer | 10:fb3542f3c5e6 | 69 | |
protongamer | 10:fb3542f3c5e6 | 70 | _command[0]= (_VM_R & 0x000000FF); |
protongamer | 10:fb3542f3c5e6 | 71 | _command[1]= (_VM_R & 0x0000FF00)>>8; |
protongamer | 10:fb3542f3c5e6 | 72 | _command[2]= (_VM_R & 0x00FF0000)>>16; |
protongamer | 10:fb3542f3c5e6 | 73 | _command[3]= (_VM_R & 0xFF000000)>>24; |
protongamer | 10:fb3542f3c5e6 | 74 | send(RPDO1_R, _command, 'D', 4); // Controle moteur droit |
protongamer | 10:fb3542f3c5e6 | 75 | } |
protongamer | 10:fb3542f3c5e6 | 76 | _rvx = _VM_R; |
protongamer | 10:fb3542f3c5e6 | 77 | _rvy = _VM_L; |
protongamer | 10:fb3542f3c5e6 | 78 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
protongamer | 10:fb3542f3c5e6 | 79 | |
protongamer | 10:fb3542f3c5e6 | 80 | |
protongamer | 10:fb3542f3c5e6 | 81 | |
protongamer | 10:fb3542f3c5e6 | 82 | } |
protongamer | 10:fb3542f3c5e6 | 83 | |
protongamer | 10:fb3542f3c5e6 | 84 | void RAC::readPosition(double *cX, double *cY, bool *t_mode){ |
protongamer | 10:fb3542f3c5e6 | 85 | |
protongamer | 10:fb3542f3c5e6 | 86 | //Left motor |
protongamer | 10:fb3542f3c5e6 | 87 | send(TPDO1_L, _command, 'R', 0); //Ask position for second length |
protongamer | 10:fb3542f3c5e6 | 88 | if(can.read(rmsg)){ |
protongamer | 10:fb3542f3c5e6 | 89 | _p_actual = rmsg.data[3]; |
protongamer | 10:fb3542f3c5e6 | 90 | _p_actual = _p_actual << 8; |
protongamer | 10:fb3542f3c5e6 | 91 | _p_actual = _p_actual | rmsg.data[2]; |
protongamer | 10:fb3542f3c5e6 | 92 | _p_actual = _p_actual << 8; |
protongamer | 10:fb3542f3c5e6 | 93 | _p_actual = _p_actual | rmsg.data[1]; |
protongamer | 10:fb3542f3c5e6 | 94 | _p_actual = _p_actual << 8; |
protongamer | 10:fb3542f3c5e6 | 95 | _p_actual = _p_actual | rmsg.data[0]; |
protongamer | 10:fb3542f3c5e6 | 96 | MOTOR_L_COM = 1; |
protongamer | 10:fb3542f3c5e6 | 97 | } |
protongamer | 10:fb3542f3c5e6 | 98 | wait(0.045); |
protongamer | 10:fb3542f3c5e6 | 99 | //Right motor |
protongamer | 10:fb3542f3c5e6 | 100 | send(TPDO1_R, _command, 'R', 0); //Ask position for first length |
protongamer | 10:fb3542f3c5e6 | 101 | if(can.read(rmsg)){ |
protongamer | 10:fb3542f3c5e6 | 102 | _p_actual2 = rmsg.data[3]; |
protongamer | 10:fb3542f3c5e6 | 103 | _p_actual2 = _p_actual2 << 8; |
protongamer | 10:fb3542f3c5e6 | 104 | _p_actual2 = _p_actual2 | rmsg.data[2]; |
protongamer | 10:fb3542f3c5e6 | 105 | _p_actual2 = _p_actual2 << 8; |
protongamer | 10:fb3542f3c5e6 | 106 | _p_actual2 = _p_actual2 | rmsg.data[1]; |
protongamer | 10:fb3542f3c5e6 | 107 | _p_actual2 = _p_actual2 << 8; |
protongamer | 10:fb3542f3c5e6 | 108 | _p_actual2 = _p_actual2 | rmsg.data[0]; |
protongamer | 10:fb3542f3c5e6 | 109 | MOTOR_R_COM = 1; |
protongamer | 10:fb3542f3c5e6 | 110 | } |
protongamer | 10:fb3542f3c5e6 | 111 | |
protongamer | 10:fb3542f3c5e6 | 112 | |
protongamer | 10:fb3542f3c5e6 | 113 | if(_BPO == 0){ |
protongamer | 10:fb3542f3c5e6 | 114 | _p_pom = _p_actual; //Refresh first origin value |
protongamer | 10:fb3542f3c5e6 | 115 | _p_pom2 = _p_actual2; //Refresh second origin value |
protongamer | 10:fb3542f3c5e6 | 116 | *t_mode = !*t_mode; //Change mode |
protongamer | 10:fb3542f3c5e6 | 117 | wait(1.0); |
protongamer | 10:fb3542f3c5e6 | 118 | } |
protongamer | 10:fb3542f3c5e6 | 119 | *t_mode = *t_mode; //if nothing change |
protongamer | 10:fb3542f3c5e6 | 120 | |
protongamer | 10:fb3542f3c5e6 | 121 | //Calculate positions values in counters |
protongamer | 10:fb3542f3c5e6 | 122 | _p1 = _p_pom - _p_actual; |
protongamer | 10:fb3542f3c5e6 | 123 | _p2 = _p_pom2 - _p_actual2; |
protongamer | 10:fb3542f3c5e6 | 124 | |
protongamer | 10:fb3542f3c5e6 | 125 | |
protongamer | 10:fb3542f3c5e6 | 126 | //Calculate Coordinates |
protongamer | 10:fb3542f3c5e6 | 127 | _l1 = LENGTH_L1_PO - (_p1/REFERENCE); //Length 1 |
protongamer | 10:fb3542f3c5e6 | 128 | _l2 = LENGTH_L2_PO - (_p2/REFERENCE); //Length 2 |
protongamer | 10:fb3542f3c5e6 | 129 | _lx = LENGTH_MOTOR; |
protongamer | 10:fb3542f3c5e6 | 130 | _dx = (((double) _l1*(double) _l1) - ((double) _l2*(double) _l2) + ((double) _lx*(double) _lx))/(2*(double) _lx); |
protongamer | 10:fb3542f3c5e6 | 131 | _dy = sqrt(((double) _l1*(double) _l1) - (_dx*_dx)); |
protongamer | 10:fb3542f3c5e6 | 132 | *cX = _dx; |
protongamer | 10:fb3542f3c5e6 | 133 | *cY = _dy; |
protongamer | 10:fb3542f3c5e6 | 134 | MOTOR_L_COM = 0; |
protongamer | 10:fb3542f3c5e6 | 135 | MOTOR_R_COM = 0; |
protongamer | 10:fb3542f3c5e6 | 136 | |
protongamer | 10:fb3542f3c5e6 | 137 | |
protongamer | 10:fb3542f3c5e6 | 138 | } |
protongamer | 10:fb3542f3c5e6 | 139 | |
protongamer | 10:fb3542f3c5e6 | 140 | |
protongamer | 10:fb3542f3c5e6 | 141 | void RAC::readVelocity(double *rvx, double *rvy) |
protongamer | 10:fb3542f3c5e6 | 142 | { |
protongamer | 10:fb3542f3c5e6 | 143 | //return velocities |
protongamer | 10:fb3542f3c5e6 | 144 | *rvx = _rvx; |
protongamer | 10:fb3542f3c5e6 | 145 | *rvy = _rvy; |
protongamer | 10:fb3542f3c5e6 | 146 | |
protongamer | 10:fb3542f3c5e6 | 147 | } |
protongamer | 10:fb3542f3c5e6 | 148 | |
protongamer | 10:fb3542f3c5e6 | 149 | |
protongamer | 10:fb3542f3c5e6 | 150 | |
protongamer | 10:fb3542f3c5e6 | 151 | void RAC::send(int id, char octet_emis[], char RouD, char longueur ) |
protongamer | 10:fb3542f3c5e6 | 152 | { |
protongamer | 10:fb3542f3c5e6 | 153 | if (RouD == 'D') |
protongamer | 10:fb3542f3c5e6 | 154 | { |
protongamer | 10:fb3542f3c5e6 | 155 | can.write(CANMessage(id, octet_emis, longueur, CANData, CANStandard )) ; |
protongamer | 10:fb3542f3c5e6 | 156 | } |
protongamer | 10:fb3542f3c5e6 | 157 | else |
protongamer | 10:fb3542f3c5e6 | 158 | { |
protongamer | 10:fb3542f3c5e6 | 159 | can.write(CANMessage(id, octet_emis, longueur, CANRemote, CANStandard )); |
protongamer | 10:fb3542f3c5e6 | 160 | } |
protongamer | 10:fb3542f3c5e6 | 161 | } |
protongamer | 10:fb3542f3c5e6 | 162 |