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

main.cpp@10:fb3542f3c5e6, 2018-05-28 (annotated)

Committer:
protongamer
Date:
Mon May 28 14:18:32 2018 +0000
Revision:
10:fb3542f3c5e6
Parent:
9:0f63d4cb5613
Child:
11:0fa23c2dab76
Version 3.5;

Who changed what in which revision?

UserRevisionLine numberNew 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
protongamer 8:1e6d3d9ae1d3 144
Ringslev 5:d5a021bbe81b 145 while(1)
Ringslev 5:d5a021bbe81b 146 {
protongamer 7:e87feff62bfd 147
protongamer 10:fb3542f3c5e6 148 /*
protongamer 7:e87feff62bfd 149 pc.printf("L1 = %d", l1);
protongamer 10:fb3542f3c5e6 150 pc.printf("\t L2 = %d", l2);*/
protongamer 10:fb3542f3c5e6 151 if(mode == DEGRAD){
protongamer 10:fb3542f3c5e6 152 pc.printf("mode = D");
protongamer 10:fb3542f3c5e6 153 }
protongamer 10:fb3542f3c5e6 154 else if(mode == NORMAL){
protongamer 10:fb3542f3c5e6 155 pc.printf("mode = N");
protongamer 10:fb3542f3c5e6 156 }
protongamer 10:fb3542f3c5e6 157 pc.printf(" x = %f", dx);
protongamer 10:fb3542f3c5e6 158 pc.printf(" y = %f", dy);
protongamer 10:fb3542f3c5e6 159 pc.printf(" VM_L = %f", pVx);
protongamer 10:fb3542f3c5e6 160 pc.printf(" VM_R = %f \r\n", pVy);
protongamer 7:e87feff62bfd 161
protongamer 10:fb3542f3c5e6 162
protongamer 10:fb3542f3c5e6 163
protongamer 10:fb3542f3c5e6 164 robot.readButtons(SWH, SWV, BPO); //read state buttons, important for others functions
protongamer 10:fb3542f3c5e6 165
protongamer 10:fb3542f3c5e6 166 CYCLE_TIME = 0;
protongamer 10:fb3542f3c5e6 167 robot.readPosition(&dx, &dy, &mode); //read motors encoders and calculate coordinates position (x,y)
protongamer 10:fb3542f3c5e6 168 robot.setVelocity(joystick_x.read(),joystick_y.read(),joystick_ctr.read(), mode); //Motors control with the joystick and with the mode we use
protongamer 10:fb3542f3c5e6 169 robot.readVelocity(&pVx, &pVy);//read velocities applied on the robot
Ringslev 5:d5a021bbe81b 170 display_mode(); // Controle des led affichant le mode
protongamer 10:fb3542f3c5e6 171
protongamer 10:fb3542f3c5e6 172 CYCLE_TIME = 1;
protongamer 1:4d70e593345f 173
protongamer 1:4d70e593345f 174 }// fin while(1)
protongamer 1:4d70e593345f 175
protongamer 1:4d70e593345f 176 } // fin main
protongamer 4:3fd7c53d31c1 177
protongamer 4:3fd7c53d31c1 178
protongamer 4:3fd7c53d31c1 179
protongamer 1:4d70e593345f 180
protongamer 3:86e21a36eecb 181