File content as of revision 12:98a051edb19a:

#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
    
    char Xtraj[] = "0.500";
    char Ytraj[] = "0.500";
    char lectpc='i';
    bool contrTraj=false;
                
    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 = %d", (int)dx);
            pc.printf(" y = %d", (int)dy);
            pc.printf(" VM_L = %d", (int)pVx);
            pc.printf(" VM_R = %d \r\n", (int)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)
        if(pc.readable()||contrTraj){
            lectpc=pc.getc();
            if(lectpc!='f'){
                contrTraj=true;
                lectpc=pc.getc();
                if(lectpc=='h'){
                    robot.setVelocity(0.50,0.60,0.5, mode);   //Motors control with the joystick and with the mode we use
                };
                if(lectpc=='b'){
                    robot.setVelocity(0.50,0.40,0.5, mode);   //Motors control with the joystick and with the mode we use
                };
                if(lectpc=='g'){
                    robot.setVelocity(0.60,0.50,0.5, mode);   //Motors control with the joystick and with the mode we use
                };
                if(lectpc=='d'){
                    robot.setVelocity(0.40,0.50,0.5, mode);   //Motors control with the joystick and with the mode we use
                };
                if(lectpc=='0'){
                    for(int i=1;i!=5;i++){
                        Xtraj[i]=pc.getc();   
                    }
                    for(int i=0;i!=5;i++){
                        Ytraj[i]=pc.getc();   
                    }
                    robot.setVelocity(atof(Xtraj),atof(Ytraj),0.5,mode);
                };    
                
            }else{
                robot.setVelocity(0.50,0.50,0.5,mode);
                contrTraj=false;
            }
        }else{
            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