File content as of revision 4:3fd7c53d31c1:

#include "mbed.h"
#include "can_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);



CAN can1(p9, p10,1000000);// on definit pin et debit
CAN can2(p30, p29, 1000000);
Serial pc(USBTX, USBRX);
//AnalogIn   pot_1(p19); // potard pour selection


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");
    } */
}




uint64_t timer_process_read; //counter to make mbed read actual position
uint64_t timer_process_ping; //counter to make mbed read actual position
float dy = 525000;//valeur à la POM
int64_t d_old, d_new;
bool first_time = 0;
 
int main() {
    
    
    CANMessage msg;
    uint8_t ping = 0; //variable pour etat de processus
    uint8_t done = 0;
    char command[8]; //word to send command
    
    
        //define Pullup switch
        SWH.mode(PullUp);
        SWV.mode(PullUp);
        BPO.mode(PullUp);
        pc.printf("starting...\r\n");
            MOD_DEG = 1;
            wait(0.1);
            MOD_DEG = 0;
            wait(0.1);
            MOD_DEG = 1;
            wait(0.1);
            MOD_DEG = 0;
            wait(0.1);
            MOD_DEG = 1;
            wait(0.1);
            MOD_DEG = 0;
            wait(0.1);
        
        command[0] = DRIVER_R1;
        command[1] = DRIVER_R2;
        
        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);
            
            }
            MOD_NOR = 1;
            for(int working = 0; working < 50; working++){
            send(TPDO1_R, command, 'R', 0); //Ask position
                timer_process_read = 0;
                        d_new = msg.data[3];
                        d_new = d_new << 8;
                        d_new = d_new | msg.data[2];
                        d_new = d_new << 8;
                        d_new = d_new | msg.data[1];
                        d_new = d_new << 8;
                        d_new = d_new | msg.data[0];
                        pc.printf("%d \r\n",d_new);}
                        d_old = d_new;
                
       
        
    while(1){
            
            //timer_process_ping++;
            timer_process_read++;
            pc.printf("%d \t",d_new);
            pc.printf("%d \t",d_old);
            //pc.printf("%d \t",d_new);
            pc.printf("%f \r\n",dy);
            
            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);
            
            }
                    
                    }

                }*/
            
            if(timer_process_read >= 10){
                send(TPDO1_R, command, 'R', 0); //Ask position
                timer_process_read = 0;
                        d_new = msg.data[3];
                        d_new = d_new << 8;
                        d_new = d_new | msg.data[2];
                        d_new = d_new << 8;
                        d_new = d_new | msg.data[1];
                        d_new = d_new << 8;
                        d_new = d_new | msg.data[0];
                     
                       /* if(first_time == 0){
                        d_old = d_new;
                    first_time = 1;
                    }*/
                }
            

            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)

            
        switch(BPO){
            case 0:
            if(done == 0){
            timer_process_read = 50;
            //timer_process_ping = 0;
            switch(SWV){
                case 0:
                command[0] = 0x00;
                command[1] = 0x8F;
                command[2] = 0x00;
                command[3] = 0x00;
                send(RPDO1_R, command, 'D', 4); //send word to put pluto driver in operational mode
                break;
                
                case 1:
                command[0] = 0xFF;
                command[1] = 0x70;
                command[2] = 0xFF;
                command[3] = 0xFF;
                send(RPDO1_R, command, 'D', 4); //send word to put pluto driver in operational mode
       

                break;
                }//end of switch SWV
                done = 1;
                }
            
            break;
            
            case 1:
                if(done == 1){
                command[0] = 0x00;
                command[1] = 0x00;
                command[2] = 0x00;
                command[3] = 0x00;
                send(RPDO1_R, command, 'D', 4); //send word to put pluto driver in operational mode
                done = 0;
                //send(TPDO1_R, command, 'R', 0); //send word to put pluto driver in operational mode
                }
            break;
                 
                }
        
            
    }// fin while(1)
         
} // fin main