Alexandre Benard
added serial up, down, left and right control
Fork of
Robot_a_cables_v3_5
by 
RaC2018
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