![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
version 3.0
Dependencies: mbed
Fork of Robot_a_cables_v2_6_5 by
Diff: main.cpp
- Revision:
- 4:3fd7c53d31c1
- Parent:
- 3:86e21a36eecb
- Child:
- 5:d5a021bbe81b
--- a/main.cpp Tue Mar 27 09:54:04 2018 +0000 +++ b/main.cpp Mon Apr 23 08:44:10 2018 +0000 @@ -22,17 +22,21 @@ 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] ); @@ -51,12 +55,22 @@ + +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; + 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); @@ -83,64 +97,145 @@ send(INITOP, command, 'D', 2); //send word to put pluto driver in operational mode MOD_NOR = 1; if(can2.read(msg)) { - pc.printf("Done\r\n"); - ping = 1; - } + 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){ - if(SWH == 0){ - wait(1.0); - send(0x0195, command, 'R', 0); //send word to put pluto driver in operational mode - wait(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(len) - }//end of SWH == 0 + }//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] = 0x00; - command[2] = 0xBF; + 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] = 0xFF; - command[2] = 0x40; + 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; } @@ -149,5 +244,8 @@ }// fin while(1) } // fin main + + + \ No newline at end of file