RaC2018 / Mbed 2 deprecated Robot_a_cables_v3_7

added serial up, down, left and right control

Dependencies:   mbed

Fork of Robot_a_cables_v3_6 by RaC2018

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?

UserRevisionLine numberNew 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