main.cpp

00001 #include "mbed.h"
00002 #include "parameters.h"
00003 #include "rac.h"
00004  /* 
00005   Projet BTS SN - Robot à câbles - Lycée Georges Cabanis
00006   Enzo Niro - Erwin Sazio
00007   Transmission par Bus CAN(protocol CANOpen)
00008   
00009  */
00010  
00011  #define DEGRAD   0
00012  #define NORMAL   1
00013  
00014 
00015 
00016 
00017 
00018 DigitalOut MOD_DEG(p25);
00019 DigitalOut MOD_NOR(p26);
00020 DigitalOut CYCLE_TIME(p20);
00021 DigitalIn SWH(p5);
00022 DigitalIn SWV(p6);
00023 DigitalIn BPO(p7);
00024 AnalogIn joystick_ctr(p15);   // Pin 2 (fil vert) Center Tap Reference
00025 AnalogIn joystick_x(p16);     // Pin 4 (fil jaune)
00026 AnalogIn joystick_y(p17);     // Pin 5 (fil bleu)
00027 
00028 RAC robot;
00029 CAN can2(p30, p29, 1000000);    // on definit pin et debit
00030 CANMessage msg;
00031 Serial pc(USBTX, USBRX, 115200);
00032 
00033 bool mode = 0;   // 0 = Degraded   1 = Normal
00034 int x,y,ctr_x,ctr_y; // Variables de joystick
00035 uint8_t ping = 0; //variable pour etat de processus
00036 uint8_t done = 0;
00037 char command[8]; //word to send command
00038 double dy = 1262, dx = 492.5;//coordinates values when origin is done
00039 //these positions are in counters
00040 int32_t p1, p2; //motors positions
00041 int32_t p_actual, p_actual2, p_pom, p_pom2; //p_actual position read by can, p_pom position done when origin is made
00042 
00043 //longueur câbles
00044 //lx = longueur entre les point des moteurs
00045 //l1 = longueur moteur gauche à l'effecteur
00046 //l2 = longueur moteur droite à l'effecteur
00047 int32_t lx, l1, l2;
00048 double pVx, pVy; // variables pour afficher les vélocitées appliquées sur le moniteur
00049 
00050 
00051 
00052 
00053 void display_mode()
00054 {
00055     if(mode == DEGRAD)   // MODE DEGRADE
00056     {
00057         MOD_DEG = 1;
00058         MOD_NOR = 0;
00059     }
00060     else if(mode == NORMAL) // MODE NORMAL
00061     {
00062         MOD_DEG = 0;
00063         MOD_NOR = 1;
00064     }
00065 }
00066 
00067 
00068 
00069 void initialisation()
00070 {
00071     pc.printf("starting...\r\n");
00072     MOD_DEG = 1;
00073     MOD_NOR = 0;
00074     wait(0.1);
00075     MOD_DEG = 0;
00076     MOD_NOR = 1;
00077     wait(0.1);
00078     MOD_DEG = 1;
00079     MOD_NOR = 0;
00080     wait(0.1);
00081     MOD_DEG = 0;
00082     MOD_NOR = 1;
00083     wait(0.1);
00084     MOD_DEG = 1;
00085     MOD_NOR = 0;
00086     wait(0.1);
00087     MOD_DEG = 0;
00088     MOD_NOR = 0;
00089     wait(0.1);
00090         
00091     command[0] = DRIVER_R1;
00092     command[1] = DRIVER_R2;
00093         
00094     while(ping == 0)    // INIT MOTEUR DROIT
00095     {
00096         pc.printf("ping droite ...\r\n");
00097         robot.send(INITOP, command, 'D', 2); //send word to put pluto driver in operational mode
00098         MOD_NOR = 1;
00099         if(can2.read(msg))
00100         {
00101         for(int i = 0; i < msg.len; i++){
00102         pc.printf("0x%x ",msg.data[i]);
00103         ping = 1;
00104         }//end of for
00105         pc.printf("\r \n");
00106         }//can.read(msg)
00107         wait(0.1);
00108         MOD_NOR = 0;
00109         wait(0.9);
00110     }
00111         
00112         ping = 0;
00113         command[0] = DRIVER_L1;
00114         command[1] = DRIVER_L2;
00115         
00116         while(ping == 0)    // INIT MOTEUR GAUCHE
00117         {
00118             pc.printf("ping gauche ...\r\n");
00119             robot.send(INITOP, command, 'D', 2); //send word to put pluto driver in operational mode
00120             MOD_DEG = 1;
00121             if(can2.read(msg)) {
00122             for(int i = 0; i < msg.len; i++){
00123             pc.printf("0x%x ",msg.data[i]);
00124             ping = 1;
00125             }//end of for
00126             pc.printf("\r \n");
00127             }//can.read(msg)
00128             wait(0.1);
00129             MOD_DEG = 0;
00130             wait(0.9);
00131         }
00132 }   // fin initialisation
00133 
00134 
00135  
00136 int main() {
00137     
00138     //define Pullup switch
00139     SWH.mode(PullUp);
00140     SWV.mode(PullUp);
00141     BPO.mode(PullUp);
00142     
00143     initialisation();   // Mise en mode opérationnel des moteurs
00144                 
00145     while(1)
00146     {
00147   
00148     /*
00149             pc.printf("L1 = %d", l1);
00150             pc.printf("\t L2 = %d", l2);*/
00151             if(mode == DEGRAD){
00152             pc.printf("mode = D");    
00153                 }
00154             else if(mode == NORMAL){
00155             pc.printf("mode = N");    
00156                 }
00157             pc.printf(" x = %f", dx);
00158             pc.printf(" y = %f", dy);
00159             pc.printf(" VM_L = %f", pVx);
00160             pc.printf(" VM_R = %f \r\n", pVy);
00161             
00162             
00163             
00164         robot.readButtons(SWH, SWV, BPO);   //read state buttons, important for others functions
00165         
00166         CYCLE_TIME = 0;  
00167         robot.readPosition(&dx, &dy, &mode); //read motors encoders and calculate coordinates position (x,y)
00168         robot.setVelocity(joystick_x.read(),joystick_y.read(),joystick_ctr.read(), mode);   //Motors control with the joystick and with the mode we use
00169         robot.readVelocity(&pVx, &pVy);//read velocities applied on the robot
00170         display_mode();     // Controle des led affichant le mode
00171             
00172         CYCLE_TIME = 1;
00173         
00174     }// fin while(1)
00175          
00176 } // fin main
00177 
00178 
00179 
00180          
00181