![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
added serial up, down, left and right control
Fork of Robot_a_cables_v3_5 by
main.cpp
- Committer:
- protongamer
- Date:
- 2018-05-28
- Revision:
- 10:fb3542f3c5e6
- Parent:
- 9:0f63d4cb5613
- Child:
- 11:0fa23c2dab76
File content as of revision 10:fb3542f3c5e6:
#include "mbed.h" #include "parameters.h" #include "rac.h" /* Projet BTS SN - Robot à câbles - Lycée Georges Cabanis Enzo Niro - Erwin Sazio Transmission par Bus CAN(protocol CANOpen) */ #define DEGRAD 0 #define NORMAL 1 DigitalOut MOD_DEG(p25); DigitalOut MOD_NOR(p26); DigitalOut CYCLE_TIME(p20); DigitalIn SWH(p5); DigitalIn SWV(p6); DigitalIn BPO(p7); AnalogIn joystick_ctr(p15); // Pin 2 (fil vert) Center Tap Reference AnalogIn joystick_x(p16); // Pin 4 (fil jaune) AnalogIn joystick_y(p17); // Pin 5 (fil bleu) RAC robot; CAN can2(p30, p29, 1000000); // on definit pin et debit CANMessage msg; Serial pc(USBTX, USBRX, 115200); bool mode = 0; // 0 = Degraded 1 = Normal int x,y,ctr_x,ctr_y; // Variables de joystick uint8_t ping = 0; //variable pour etat de processus uint8_t done = 0; char command[8]; //word to send command double dy = 1262, dx = 492.5;//coordinates values when origin is done //these positions are in counters int32_t p1, p2; //motors positions int32_t p_actual, p_actual2, p_pom, p_pom2; //p_actual position read by can, p_pom position done when origin is made //longueur câbles //lx = longueur entre les point des moteurs //l1 = longueur moteur gauche à l'effecteur //l2 = longueur moteur droite à l'effecteur int32_t lx, l1, l2; double pVx, pVy; // variables pour afficher les vélocitées appliquées sur le moniteur void display_mode() { if(mode == DEGRAD) // MODE DEGRADE { MOD_DEG = 1; MOD_NOR = 0; } else if(mode == NORMAL) // MODE NORMAL { MOD_DEG = 0; MOD_NOR = 1; } } void initialisation() { pc.printf("starting...\r\n"); MOD_DEG = 1; MOD_NOR = 0; wait(0.1); MOD_DEG = 0; MOD_NOR = 1; wait(0.1); MOD_DEG = 1; MOD_NOR = 0; wait(0.1); MOD_DEG = 0; MOD_NOR = 1; wait(0.1); MOD_DEG = 1; MOD_NOR = 0; wait(0.1); MOD_DEG = 0; MOD_NOR = 0; wait(0.1); command[0] = DRIVER_R1; command[1] = DRIVER_R2; while(ping == 0) // INIT MOTEUR DROIT { pc.printf("ping droite ...\r\n"); robot.send(INITOP, command, 'D', 2); //send word to put pluto driver in operational mode MOD_NOR = 1; if(can2.read(msg)) { for(int i = 0; i < msg.len; i++){ pc.printf("0x%x ",msg.data[i]); ping = 1; }//end of for pc.printf("\r \n"); }//can.read(msg) wait(0.1); MOD_NOR = 0; wait(0.9); } ping = 0; command[0] = DRIVER_L1; command[1] = DRIVER_L2; while(ping == 0) // INIT MOTEUR GAUCHE { pc.printf("ping gauche ...\r\n"); robot.send(INITOP, command, 'D', 2); //send word to put pluto driver in operational mode MOD_DEG = 1; if(can2.read(msg)) { for(int i = 0; i < msg.len; i++){ pc.printf("0x%x ",msg.data[i]); ping = 1; }//end of for pc.printf("\r \n"); }//can.read(msg) wait(0.1); MOD_DEG = 0; wait(0.9); } } // fin initialisation int main() { //define Pullup switch SWH.mode(PullUp); SWV.mode(PullUp); BPO.mode(PullUp); initialisation(); // Mise en mode opérationnel des moteurs while(1) { /* pc.printf("L1 = %d", l1); pc.printf("\t L2 = %d", l2);*/ if(mode == DEGRAD){ pc.printf("mode = D"); } else if(mode == NORMAL){ pc.printf("mode = N"); } pc.printf(" x = %f", dx); pc.printf(" y = %f", dy); pc.printf(" VM_L = %f", pVx); pc.printf(" VM_R = %f \r\n", pVy); robot.readButtons(SWH, SWV, BPO); //read state buttons, important for others functions CYCLE_TIME = 0; robot.readPosition(&dx, &dy, &mode); //read motors encoders and calculate coordinates position (x,y) robot.setVelocity(joystick_x.read(),joystick_y.read(),joystick_ctr.read(), mode); //Motors control with the joystick and with the mode we use robot.readVelocity(&pVx, &pVy);//read velocities applied on the robot display_mode(); // Controle des led affichant le mode CYCLE_TIME = 1; }// fin while(1) } // fin main