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