File content as of revision 7:e87feff62bfd:

#include "mbed.h"
#include "parameters.h"
 /* 
  Projet BTS SN - Robot à câbles - Lycée Georges Cabanis
  Enzo Niro - Erwin Sazio
  Transmission par Bus CAN(protocol CANOpen)
  
 */
 
 

DigitalOut MOTOR1_OP(LED1);
DigitalOut MOTOR2_OP(LED2);
DigitalOut DEB_MOD_DEG(LED3);
DigitalOut DEB_MOD_NOR(LED4);
DigitalOut MOD_DEG(p25);
DigitalOut MOD_NOR(p26);
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)

CAN can2(p30, p29, 1000000);    // on definit pin et debit
CANMessage msg;
Serial pc(USBTX, USBRX);

uint8_t mode = 0;
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
uint16_t timer_process_read; //counter to make mbed read actual position
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;
 




void display_mode()
{
    if(mode == 0)   // MODE DEGRADE
    {
        MOD_DEG = 1;
        MOD_NOR = 0;
    }
    else if(mode == 1) // MODE NORMAL
    {
        MOD_DEG = 0;
        MOD_NOR = 1;
    }
}

void send(int id, char octet_emis[], char RouD, char longueur )
{
    int emis_ok = 0 ;
    
    MOTOR1_OP = 1;
    MOTOR1_OP = 0;
    
    if (RouD == 'D')
    { emis_ok = can2.write(CANMessage(id, octet_emis, longueur, CANData, CANStandard )) ;
      //pc.printf("id: %x, %c L = %d,  \nData : %x:%x:%x:%x ...   \n", id,RouD,longueur,octet_emis[0],octet_emis[1], octet_emis[2], octet_emis[3] );
    }// c'ets la valeur retournée par la fonction write
   else
    { emis_ok = can2.write(CANMessage(id, octet_emis, longueur,  CANRemote, CANStandard ));
        //pc.printf("id: %x, %c L = %d,  \nData : %x:%x:%x:%x ...   \n", id,RouD,longueur,octet_emis[0],octet_emis[1], octet_emis[2], octet_emis[3] );
         }
    //lcd.locate(0,10);
    //lcd.printf("id: %x, %c L = %d,  \nData : %x:%x:%x:%x ...   \n", id,RouD,longueur,octet_emis[0],octet_emis[1], octet_emis[2], octet_emis[3] );
    /*if(emis_ok) {
        // ici octet emis n'a pas de sens car trame remote !
     //pc.printf("send \r \n");
    } */
}

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");
        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");
            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



void read_position(){
    
     
                        
                        
                        

                     
                    
    
    
 
    
    }//fin de fonction



void control_Moteur()
{
    ///////////// CONTROLE MOTEUR //////////////////////////////////////////////////////////////////////////////
    // LECTURE Joystick + stockage valeurs dans x, y
    ctr_x = int(-((joystick_ctr.read()*100)-50));
    ctr_y = int((joystick_ctr.read()*100)-50);
    if(SWH){x = int(-((joystick_x.read()*100)-50))+ctr_x;}else{x=0;}    //  Detection x avec correction ctr de -42 à +42
    if(SWV){y = int((joystick_y.read()*100)-50)+ctr_y;}else{y=0;}       //  Detection y avec correction ctr
        
    if(mode ==0)
    {
    ///////////// MODE DEGRADE /////////////////////////////////////
    float VX = (float(x)/42)*425000;    // Calcule brut vélocité x, y
    float VY = (float(y)/42)*425000;    
    long int VM_L = VX - VY;            // Calcule des vélocités exactes de chaques moteurs
    long int VM_R = -VX - VY;
        
    command[0]= (VM_L & 0x000000FF);        // Masque + décalage pour translation big vers little endian
    command[1]= (VM_L & 0x0000FF00)>>8;
    command[2]= (VM_L & 0x00FF0000)>>16;
    command[3]= (VM_L & 0xFF000000)>>24;
    send(RPDO1_L, command, 'D', 4);         // Controle moteur gauche
        
    command[0]= (VM_R & 0x000000FF);
    command[1]= (VM_R & 0x0000FF00)>>8;
    command[2]= (VM_R & 0x00FF0000)>>16;
    command[3]= (VM_R & 0xFF000000)>>24;
    send(RPDO1_R, command, 'D', 4);         // Controle moteur droit
    ////////////////////////////////////////////////////////////////
    }
    else if(mode ==1)
    {
        // MODE NORMAL
    }
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////
}
 
int main() {
    
    //define Pullup switch
    SWH.mode(PullUp);
    SWV.mode(PullUp);
    BPO.mode(PullUp);
        
    initialisation();   // Mise en mode opérationnel des moteurs



                lx = LENGTH_MOTOR;
    while(1)
    {
  
        timer_process_read++;
        l1 = LENGTH_L1_PO - (p1/2545); //Length 1
        l2 = LENGTH_L2_PO - (p2/2545); //Length 2
       // dx = (pow((double) l1, 2) - pow((double) l2, 2) + pow((double) lx, 2))/(2*(double) lx);
       // dy = sqrt(pow((double) l1, 2) - pow(dx, 2));
        dx = (((double) l2*(double) l2) - ((double) l1*(double) l1) + ((double) lx*(double) lx))/(2*(double) lx);
        dy = sqrt(((double) l1*(double) l1) - (dx*dx));
        
        if(BPO == 0){                       
         timer_process_read = 0;
         p_pom = p_actual;
         p_pom2 = p_actual2;
         wait(1.0);
        }
        
        
        

         
                       
                
        
                        

            
            if(timer_process_read == 5){            
                        send(TPDO1_L, command, 'R', 0); //Ask position for second length
                        if(can2.read(msg)){
                        p_actual2 = msg.data[3];
                        p_actual2 = p_actual2 << 8;
                        p_actual2 = p_actual2 | msg.data[2];
                        p_actual2 = p_actual2 << 8;
                        p_actual2 = p_actual2 | msg.data[1];
                        p_actual2 = p_actual2 << 8;
                        p_actual2 = p_actual2 | msg.data[0];
                        }
                        
               
                        }
                        
                        if(timer_process_read >= 10){
               send(TPDO1_R, command, 'R', 0); //Ask position for first length
                if(can2.read(msg)){
                        p_actual = msg.data[3];
                        p_actual = p_actual << 8;
                        p_actual = p_actual | msg.data[2];
                        p_actual = p_actual << 8;
                        p_actual = p_actual | msg.data[1];
                        p_actual = p_actual << 8;
                        p_actual = p_actual | msg.data[0];
                        }
                        timer_process_read = 0;
                       }
                        
                        
                        
                        
                
                        
            
    
            pc.printf("L1 = %d", l1);
            pc.printf("\t L2 = %d", l2);
            pc.printf("\t x = %f", dx);
            pc.printf("\t y = %f", dy);/*
            pc.printf("\t %d", p_actual);
            pc.printf("\t %d", p_actual2);
            pc.printf("\t %d", p_pom);
            pc.printf("\t %d\r\n", p_pom2);*/
            pc.printf("\t %d", p1);
            pc.printf("\t %d \r\n", p2);
            
            
                
              p1 = p_pom - p_actual;
               p2 = p_pom2 - p_actual2;
            

            
      
            
            
        //read_position();
        control_Moteur();   // Controle du meteur mode degradé et normal
        display_mode();     // Controle des led affichant le mode
        
    }// fin while(1)
         
} // fin main