version 3.0
Dependencies: mbed
Fork of Robot_a_cables_v2_6_5 by
main.cpp
- Committer:
- protongamer
- Date:
- 2018-04-23
- Revision:
- 4:3fd7c53d31c1
- Parent:
- 3:86e21a36eecb
- Child:
- 5:d5a021bbe81b
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