File content as of revision 6:ffffa9dfadfc:

#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(p26);
DigitalOut MOD_NOR(p25);
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
uint64_t timer_process_read; //counter to make mbed read actual position
float 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_pom; //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
int64_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(){
    
     
                        
                        
                        
   /*  send(TPDO1_R, command, 'R', 0); //Ask position
                timer_process_read = 0;
                        p2 = msg.data[3];
                        p2 = p2 << 8;
                        p2 = p2 | msg.data[2];
                        p2 = p2 << 8;
                        p2 = p2 | msg.data[1];
                        p2 = p2 << 8;
                        p2 = p2 | msg.data[0];*/
                     
                    
    
    lx = 985;
    
    //l2 = LENGTH_L2_PO + (p2*1000)/2500;
    
    }//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



                
    while(1)
    {
        //timer_process_ping++;
        timer_process_read++;
        l1 = (LENGTH_L1_PO*1000) - (p1*1000)/2500; //Longueur 1
        
        if(BPO == 0){
         send(TPDO1_R, command, 'R', 0); //Ask position
               
                        p_pom = msg.data[3];
                        p_pom = p_pom << 8;
                        p_pom = p_pom | msg.data[2];
                        p_pom = p_pom << 8;
                        p_pom = p_pom | msg.data[1];
                        p_pom = p_pom << 8;
                        p_pom = p_pom | msg.data[0];
        wait(1.0);
        }
            
        //dy = 525000 + (d_new*1000)/2500;
          
                
        //wait_ms(1);
            
        /*  if(timer_process_ping >= 5000){
                send(TPDO2_R, command, 'R', 0); //Ask mod of motor
                timer_process_ping = 0;
                if(can2.read(msg)) {
            for(int i = 0; i < msg.len; i++){
            pc.printf("0x%x ",msg.data[i]);
            }//end of for
            pc.printf("\r \n");
            }//can.read(msg)
                
                if(msg.data[0] != 0x27 || msg.data[1] != 0x56){//if device is not in operational mode
             ping = 0;       
                    while(ping == 0){
            pc.printf("ping...\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);
            
            }
                    
                    }

                }*/
            pc.printf("%d", l1);
            pc.printf("\t %d", p_pom);
            pc.printf("\t %d", p_actual);
            pc.printf("\t %d \r\n", p1);
            if(timer_process_read >= 10){
               send(TPDO1_R, command, 'R', 0); //Ask position
                timer_process_read = 0;
                        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];
                        
                }
              
               p1 = p_pom - p_actual;
            

            if(can2.read(msg)) {
            for(int i = 0; i < msg.len; i++){
            //pc.printf("0x%x ",msg.data[i]);
            }//end of for
            //pc.printf("\r \n");
            }//can.read(msg)
            
        //read_position();
        control_Moteur();   // Controle du meteur mode degradé et normal
        display_mode();     // Controle des led affichant le mode
        
    }// fin while(1)
         
} // fin main