added serial up, down, left and right control
Fork of Robot_a_cables_v3_5 by
main.cpp@11:0fa23c2dab76, 2018-06-05 (annotated)
- Committer:
- unslaad
- Date:
- Tue Jun 05 07:24:15 2018 +0000
- Revision:
- 11:0fa23c2dab76
- Parent:
- 10:fb3542f3c5e6
added serial up, down, left and right control
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
LouisReynier | 0:1ab5fdb4fa56 | 1 | #include "mbed.h" |
protongamer | 6:ffffa9dfadfc | 2 | #include "parameters.h" |
protongamer | 10:fb3542f3c5e6 | 3 | #include "rac.h" |
protongamer | 3:86e21a36eecb | 4 | /* |
protongamer | 3:86e21a36eecb | 5 | Projet BTS SN - Robot à câbles - Lycée Georges Cabanis |
protongamer | 3:86e21a36eecb | 6 | Enzo Niro - Erwin Sazio |
protongamer | 3:86e21a36eecb | 7 | Transmission par Bus CAN(protocol CANOpen) |
protongamer | 3:86e21a36eecb | 8 | |
protongamer | 3:86e21a36eecb | 9 | */ |
LouisReynier | 0:1ab5fdb4fa56 | 10 | |
protongamer | 8:1e6d3d9ae1d3 | 11 | #define DEGRAD 0 |
protongamer | 8:1e6d3d9ae1d3 | 12 | #define NORMAL 1 |
protongamer | 1:4d70e593345f | 13 | |
protongamer | 10:fb3542f3c5e6 | 14 | |
protongamer | 3:86e21a36eecb | 15 | |
protongamer | 10:fb3542f3c5e6 | 16 | |
protongamer | 10:fb3542f3c5e6 | 17 | |
protongamer | 7:e87feff62bfd | 18 | DigitalOut MOD_DEG(p25); |
protongamer | 7:e87feff62bfd | 19 | DigitalOut MOD_NOR(p26); |
protongamer | 10:fb3542f3c5e6 | 20 | DigitalOut CYCLE_TIME(p20); |
protongamer | 3:86e21a36eecb | 21 | DigitalIn SWH(p5); |
protongamer | 3:86e21a36eecb | 22 | DigitalIn SWV(p6); |
protongamer | 3:86e21a36eecb | 23 | DigitalIn BPO(p7); |
Ringslev | 5:d5a021bbe81b | 24 | AnalogIn joystick_ctr(p15); // Pin 2 (fil vert) Center Tap Reference |
Ringslev | 5:d5a021bbe81b | 25 | AnalogIn joystick_x(p16); // Pin 4 (fil jaune) |
Ringslev | 5:d5a021bbe81b | 26 | AnalogIn joystick_y(p17); // Pin 5 (fil bleu) |
protongamer | 3:86e21a36eecb | 27 | |
protongamer | 10:fb3542f3c5e6 | 28 | RAC robot; |
Ringslev | 5:d5a021bbe81b | 29 | CAN can2(p30, p29, 1000000); // on definit pin et debit |
Ringslev | 5:d5a021bbe81b | 30 | CANMessage msg; |
protongamer | 10:fb3542f3c5e6 | 31 | Serial pc(USBTX, USBRX, 115200); |
protongamer | 4:3fd7c53d31c1 | 32 | |
protongamer | 10:fb3542f3c5e6 | 33 | bool mode = 0; // 0 = Degraded 1 = Normal |
Ringslev | 5:d5a021bbe81b | 34 | int x,y,ctr_x,ctr_y; // Variables de joystick |
Ringslev | 5:d5a021bbe81b | 35 | uint8_t ping = 0; //variable pour etat de processus |
Ringslev | 5:d5a021bbe81b | 36 | uint8_t done = 0; |
Ringslev | 5:d5a021bbe81b | 37 | char command[8]; //word to send command |
protongamer | 7:e87feff62bfd | 38 | double dy = 1262, dx = 492.5;//coordinates values when origin is done |
protongamer | 6:ffffa9dfadfc | 39 | //these positions are in counters |
protongamer | 6:ffffa9dfadfc | 40 | int32_t p1, p2; //motors positions |
protongamer | 7:e87feff62bfd | 41 | int32_t p_actual, p_actual2, p_pom, p_pom2; //p_actual position read by can, p_pom position done when origin is made |
protongamer | 6:ffffa9dfadfc | 42 | |
protongamer | 6:ffffa9dfadfc | 43 | //longueur câbles |
protongamer | 6:ffffa9dfadfc | 44 | //lx = longueur entre les point des moteurs |
protongamer | 6:ffffa9dfadfc | 45 | //l1 = longueur moteur gauche à l'effecteur |
protongamer | 6:ffffa9dfadfc | 46 | //l2 = longueur moteur droite à l'effecteur |
protongamer | 7:e87feff62bfd | 47 | int32_t lx, l1, l2; |
protongamer | 10:fb3542f3c5e6 | 48 | double pVx, pVy; // variables pour afficher les vélocitées appliquées sur le moniteur |
protongamer | 6:ffffa9dfadfc | 49 | |
protongamer | 6:ffffa9dfadfc | 50 | |
protongamer | 6:ffffa9dfadfc | 51 | |
protongamer | 1:4d70e593345f | 52 | |
Ringslev | 5:d5a021bbe81b | 53 | void display_mode() |
Ringslev | 5:d5a021bbe81b | 54 | { |
protongamer | 8:1e6d3d9ae1d3 | 55 | if(mode == DEGRAD) // MODE DEGRADE |
Ringslev | 5:d5a021bbe81b | 56 | { |
Ringslev | 5:d5a021bbe81b | 57 | MOD_DEG = 1; |
Ringslev | 5:d5a021bbe81b | 58 | MOD_NOR = 0; |
Ringslev | 5:d5a021bbe81b | 59 | } |
protongamer | 8:1e6d3d9ae1d3 | 60 | else if(mode == NORMAL) // MODE NORMAL |
Ringslev | 5:d5a021bbe81b | 61 | { |
Ringslev | 5:d5a021bbe81b | 62 | MOD_DEG = 0; |
Ringslev | 5:d5a021bbe81b | 63 | MOD_NOR = 1; |
Ringslev | 5:d5a021bbe81b | 64 | } |
Ringslev | 5:d5a021bbe81b | 65 | } |
protongamer | 1:4d70e593345f | 66 | |
protongamer | 10:fb3542f3c5e6 | 67 | |
protongamer | 1:4d70e593345f | 68 | |
Ringslev | 5:d5a021bbe81b | 69 | void initialisation() |
Ringslev | 5:d5a021bbe81b | 70 | { |
Ringslev | 5:d5a021bbe81b | 71 | pc.printf("starting...\r\n"); |
Ringslev | 5:d5a021bbe81b | 72 | MOD_DEG = 1; |
Ringslev | 5:d5a021bbe81b | 73 | MOD_NOR = 0; |
Ringslev | 5:d5a021bbe81b | 74 | wait(0.1); |
Ringslev | 5:d5a021bbe81b | 75 | MOD_DEG = 0; |
Ringslev | 5:d5a021bbe81b | 76 | MOD_NOR = 1; |
Ringslev | 5:d5a021bbe81b | 77 | wait(0.1); |
Ringslev | 5:d5a021bbe81b | 78 | MOD_DEG = 1; |
Ringslev | 5:d5a021bbe81b | 79 | MOD_NOR = 0; |
Ringslev | 5:d5a021bbe81b | 80 | wait(0.1); |
Ringslev | 5:d5a021bbe81b | 81 | MOD_DEG = 0; |
Ringslev | 5:d5a021bbe81b | 82 | MOD_NOR = 1; |
Ringslev | 5:d5a021bbe81b | 83 | wait(0.1); |
Ringslev | 5:d5a021bbe81b | 84 | MOD_DEG = 1; |
Ringslev | 5:d5a021bbe81b | 85 | MOD_NOR = 0; |
Ringslev | 5:d5a021bbe81b | 86 | wait(0.1); |
Ringslev | 5:d5a021bbe81b | 87 | MOD_DEG = 0; |
Ringslev | 5:d5a021bbe81b | 88 | MOD_NOR = 0; |
Ringslev | 5:d5a021bbe81b | 89 | wait(0.1); |
Ringslev | 5:d5a021bbe81b | 90 | |
protongamer | 10:fb3542f3c5e6 | 91 | command[0] = DRIVER_R1; |
Ringslev | 5:d5a021bbe81b | 92 | command[1] = DRIVER_R2; |
Ringslev | 5:d5a021bbe81b | 93 | |
Ringslev | 5:d5a021bbe81b | 94 | while(ping == 0) // INIT MOTEUR DROIT |
Ringslev | 5:d5a021bbe81b | 95 | { |
Ringslev | 5:d5a021bbe81b | 96 | pc.printf("ping droite ...\r\n"); |
protongamer | 10:fb3542f3c5e6 | 97 | robot.send(INITOP, command, 'D', 2); //send word to put pluto driver in operational mode |
Ringslev | 5:d5a021bbe81b | 98 | MOD_NOR = 1; |
Ringslev | 5:d5a021bbe81b | 99 | if(can2.read(msg)) |
Ringslev | 5:d5a021bbe81b | 100 | { |
protongamer | 10:fb3542f3c5e6 | 101 | for(int i = 0; i < msg.len; i++){ |
protongamer | 10:fb3542f3c5e6 | 102 | pc.printf("0x%x ",msg.data[i]); |
protongamer | 10:fb3542f3c5e6 | 103 | ping = 1; |
protongamer | 10:fb3542f3c5e6 | 104 | }//end of for |
protongamer | 10:fb3542f3c5e6 | 105 | pc.printf("\r \n"); |
protongamer | 10:fb3542f3c5e6 | 106 | }//can.read(msg) |
protongamer | 10:fb3542f3c5e6 | 107 | wait(0.1); |
protongamer | 10:fb3542f3c5e6 | 108 | MOD_NOR = 0; |
protongamer | 10:fb3542f3c5e6 | 109 | wait(0.9); |
protongamer | 10:fb3542f3c5e6 | 110 | } |
Ringslev | 5:d5a021bbe81b | 111 | |
protongamer | 10:fb3542f3c5e6 | 112 | ping = 0; |
Ringslev | 5:d5a021bbe81b | 113 | command[0] = DRIVER_L1; |
Ringslev | 5:d5a021bbe81b | 114 | command[1] = DRIVER_L2; |
Ringslev | 5:d5a021bbe81b | 115 | |
Ringslev | 5:d5a021bbe81b | 116 | while(ping == 0) // INIT MOTEUR GAUCHE |
Ringslev | 5:d5a021bbe81b | 117 | { |
Ringslev | 5:d5a021bbe81b | 118 | pc.printf("ping gauche ...\r\n"); |
protongamer | 10:fb3542f3c5e6 | 119 | robot.send(INITOP, command, 'D', 2); //send word to put pluto driver in operational mode |
protongamer | 3:86e21a36eecb | 120 | MOD_DEG = 1; |
protongamer | 3:86e21a36eecb | 121 | if(can2.read(msg)) { |
protongamer | 4:3fd7c53d31c1 | 122 | for(int i = 0; i < msg.len; i++){ |
protongamer | 4:3fd7c53d31c1 | 123 | pc.printf("0x%x ",msg.data[i]); |
protongamer | 4:3fd7c53d31c1 | 124 | ping = 1; |
protongamer | 4:3fd7c53d31c1 | 125 | }//end of for |
protongamer | 4:3fd7c53d31c1 | 126 | pc.printf("\r \n"); |
protongamer | 4:3fd7c53d31c1 | 127 | }//can.read(msg) |
protongamer | 3:86e21a36eecb | 128 | wait(0.1); |
Ringslev | 5:d5a021bbe81b | 129 | MOD_DEG = 0; |
protongamer | 3:86e21a36eecb | 130 | wait(0.9); |
Ringslev | 5:d5a021bbe81b | 131 | } |
Ringslev | 5:d5a021bbe81b | 132 | } // fin initialisation |
Ringslev | 5:d5a021bbe81b | 133 | |
protongamer | 6:ffffa9dfadfc | 134 | |
Ringslev | 5:d5a021bbe81b | 135 | |
Ringslev | 5:d5a021bbe81b | 136 | int main() { |
Ringslev | 5:d5a021bbe81b | 137 | |
Ringslev | 5:d5a021bbe81b | 138 | //define Pullup switch |
Ringslev | 5:d5a021bbe81b | 139 | SWH.mode(PullUp); |
Ringslev | 5:d5a021bbe81b | 140 | SWV.mode(PullUp); |
Ringslev | 5:d5a021bbe81b | 141 | BPO.mode(PullUp); |
protongamer | 10:fb3542f3c5e6 | 142 | |
Ringslev | 5:d5a021bbe81b | 143 | initialisation(); // Mise en mode opérationnel des moteurs |
unslaad | 11:0fa23c2dab76 | 144 | |
unslaad | 11:0fa23c2dab76 | 145 | char Xtraj[] = "0.500"; |
unslaad | 11:0fa23c2dab76 | 146 | char Ytraj[] = "0.500"; |
unslaad | 11:0fa23c2dab76 | 147 | char lectpc='i'; |
unslaad | 11:0fa23c2dab76 | 148 | bool contrTraj=false; |
protongamer | 8:1e6d3d9ae1d3 | 149 | |
Ringslev | 5:d5a021bbe81b | 150 | while(1) |
Ringslev | 5:d5a021bbe81b | 151 | { |
protongamer | 7:e87feff62bfd | 152 | |
protongamer | 10:fb3542f3c5e6 | 153 | /* |
protongamer | 7:e87feff62bfd | 154 | pc.printf("L1 = %d", l1); |
protongamer | 10:fb3542f3c5e6 | 155 | pc.printf("\t L2 = %d", l2);*/ |
protongamer | 10:fb3542f3c5e6 | 156 | if(mode == DEGRAD){ |
unslaad | 11:0fa23c2dab76 | 157 | pc.printf("mode = d"); |
protongamer | 10:fb3542f3c5e6 | 158 | } |
protongamer | 10:fb3542f3c5e6 | 159 | else if(mode == NORMAL){ |
unslaad | 11:0fa23c2dab76 | 160 | pc.printf("mode = n"); |
protongamer | 10:fb3542f3c5e6 | 161 | } |
protongamer | 10:fb3542f3c5e6 | 162 | pc.printf(" x = %f", dx); |
protongamer | 10:fb3542f3c5e6 | 163 | pc.printf(" y = %f", dy); |
protongamer | 10:fb3542f3c5e6 | 164 | pc.printf(" VM_L = %f", pVx); |
protongamer | 10:fb3542f3c5e6 | 165 | pc.printf(" VM_R = %f \r\n", pVy); |
protongamer | 7:e87feff62bfd | 166 | |
protongamer | 10:fb3542f3c5e6 | 167 | |
protongamer | 10:fb3542f3c5e6 | 168 | |
protongamer | 10:fb3542f3c5e6 | 169 | robot.readButtons(SWH, SWV, BPO); //read state buttons, important for others functions |
protongamer | 10:fb3542f3c5e6 | 170 | |
protongamer | 10:fb3542f3c5e6 | 171 | CYCLE_TIME = 0; |
protongamer | 10:fb3542f3c5e6 | 172 | robot.readPosition(&dx, &dy, &mode); //read motors encoders and calculate coordinates position (x,y) |
unslaad | 11:0fa23c2dab76 | 173 | if(pc.readable()||contrTraj){ |
unslaad | 11:0fa23c2dab76 | 174 | lectpc=pc.getc(); |
unslaad | 11:0fa23c2dab76 | 175 | if(lectpc!='f'){ |
unslaad | 11:0fa23c2dab76 | 176 | contrTraj=true; |
unslaad | 11:0fa23c2dab76 | 177 | lectpc=pc.getc(); |
unslaad | 11:0fa23c2dab76 | 178 | if(lectpc=='h'){ |
unslaad | 11:0fa23c2dab76 | 179 | /*for(int i=0;i!=5;i++){ |
unslaad | 11:0fa23c2dab76 | 180 | Xtraj[i]=pc.getc(); |
unslaad | 11:0fa23c2dab76 | 181 | } |
unslaad | 11:0fa23c2dab76 | 182 | for(int i=0;i!=5;i++){ |
unslaad | 11:0fa23c2dab76 | 183 | Ytraj[i]=pc.getc(); |
unslaad | 11:0fa23c2dab76 | 184 | }*/ |
unslaad | 11:0fa23c2dab76 | 185 | robot.setVelocity(0.50,0.60,0.5, mode); //Motors control with the joystick and with the mode we use |
unslaad | 11:0fa23c2dab76 | 186 | }; |
unslaad | 11:0fa23c2dab76 | 187 | if(lectpc=='b'){ |
unslaad | 11:0fa23c2dab76 | 188 | robot.setVelocity(0.50,0.40,0.5, mode); //Motors control with the joystick and with the mode we use |
unslaad | 11:0fa23c2dab76 | 189 | }; |
unslaad | 11:0fa23c2dab76 | 190 | if(lectpc=='g'){ |
unslaad | 11:0fa23c2dab76 | 191 | robot.setVelocity(0.60,0.50,0.5, mode); //Motors control with the joystick and with the mode we use |
unslaad | 11:0fa23c2dab76 | 192 | }; |
unslaad | 11:0fa23c2dab76 | 193 | if(lectpc=='d'){ |
unslaad | 11:0fa23c2dab76 | 194 | robot.setVelocity(0.40,0.50,0.5, mode); //Motors control with the joystick and with the mode we use |
unslaad | 11:0fa23c2dab76 | 195 | }; |
unslaad | 11:0fa23c2dab76 | 196 | }else{ |
unslaad | 11:0fa23c2dab76 | 197 | robot.setVelocity(0.50,0.50,0.5,mode); |
unslaad | 11:0fa23c2dab76 | 198 | contrTraj=false; |
unslaad | 11:0fa23c2dab76 | 199 | } |
unslaad | 11:0fa23c2dab76 | 200 | }else{ |
unslaad | 11:0fa23c2dab76 | 201 | robot.setVelocity(joystick_x.read(),joystick_y.read(),joystick_ctr.read(), mode); //Motors control with the joystick and with the mode we use |
unslaad | 11:0fa23c2dab76 | 202 | } |
protongamer | 10:fb3542f3c5e6 | 203 | robot.readVelocity(&pVx, &pVy);//read velocities applied on the robot |
Ringslev | 5:d5a021bbe81b | 204 | display_mode(); // Controle des led affichant le mode |
protongamer | 10:fb3542f3c5e6 | 205 | |
protongamer | 10:fb3542f3c5e6 | 206 | CYCLE_TIME = 1; |
protongamer | 1:4d70e593345f | 207 | |
protongamer | 1:4d70e593345f | 208 | }// fin while(1) |
protongamer | 1:4d70e593345f | 209 | |
protongamer | 1:4d70e593345f | 210 | } // fin main |
protongamer | 4:3fd7c53d31c1 | 211 | |
protongamer | 4:3fd7c53d31c1 | 212 | |
protongamer | 4:3fd7c53d31c1 | 213 | |
protongamer | 1:4d70e593345f | 214 | |
protongamer | 3:86e21a36eecb | 215 |