Alexandre Benard
added serial up, down, left and right control
Fork of
Robot_a_cables_v3_5
by 
RaC2018
Diff: main.cpp
- Revision:
- 7:e87feff62bfd
- Parent:
- 6:ffffa9dfadfc
- Child:
- 8:1e6d3d9ae1d3
--- a/main.cpp Mon May 07 12:12:34 2018 +0000
+++ b/main.cpp Mon May 14 15:27:10 2018 +0000
@@ -13,8 +13,8 @@
DigitalOut MOTOR2_OP(LED2);
DigitalOut DEB_MOD_DEG(LED3);
DigitalOut DEB_MOD_NOR(LED4);
-DigitalOut MOD_DEG(p26);
-DigitalOut MOD_NOR(p25);
+DigitalOut MOD_DEG(p25);
+DigitalOut MOD_NOR(p26);
DigitalIn SWH(p5);
DigitalIn SWV(p6);
DigitalIn BPO(p7);
@@ -31,17 +31,17 @@
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
+uint16_t timer_process_read; //counter to make mbed read actual position
+double 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
+int32_t p_actual, p_actual2, p_pom, p_pom2; //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;
+int32_t lx, l1, l2;
@@ -158,21 +158,12 @@
- /* 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
@@ -226,70 +217,53 @@
-
+ lx = LENGTH_MOTOR;
while(1)
{
- //timer_process_ping++;
+
timer_process_read++;
- l1 = (LENGTH_L1_PO*1000) - (p1*1000)/2500; //Longueur 1
+ l1 = LENGTH_L1_PO - (p1/2545); //Length 1
+ l2 = LENGTH_L2_PO - (p2/2545); //Length 2
+ // dx = (pow((double) l1, 2) - pow((double) l2, 2) + pow((double) lx, 2))/(2*(double) lx);
+ // dy = sqrt(pow((double) l1, 2) - pow(dx, 2));
+ dx = (((double) l2*(double) l2) - ((double) l1*(double) l1) + ((double) lx*(double) lx))/(2*(double) lx);
+ dy = sqrt(((double) l1*(double) l1) - (dx*dx));
- 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);
+ if(BPO == 0){
+ timer_process_read = 0;
+ p_pom = p_actual;
+ p_pom2 = p_actual2;
+ 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;
+ if(timer_process_read == 5){
+ send(TPDO1_L, command, 'R', 0); //Ask position for second length
+ if(can2.read(msg)){
+ p_actual2 = msg.data[3];
+ p_actual2 = p_actual2 << 8;
+ p_actual2 = p_actual2 | msg.data[2];
+ p_actual2 = p_actual2 << 8;
+ p_actual2 = p_actual2 | msg.data[1];
+ p_actual2 = p_actual2 << 8;
+ p_actual2 = p_actual2 | msg.data[0];
+ }
+
+
+ }
+
+ if(timer_process_read >= 10){
+ send(TPDO1_R, command, 'R', 0); //Ask position for first length
+ if(can2.read(msg)){
p_actual = msg.data[3];
p_actual = p_actual << 8;
p_actual = p_actual | msg.data[2];
@@ -297,18 +271,37 @@
p_actual = p_actual | msg.data[1];
p_actual = p_actual << 8;
p_actual = p_actual | msg.data[0];
+ }
+ timer_process_read = 0;
+ }
- }
-
- p1 = p_pom - p_actual;
+
+
+
+
+
+
+
+ pc.printf("L1 = %d", l1);
+ pc.printf("\t L2 = %d", l2);
+ pc.printf("\t x = %f", dx);
+ pc.printf("\t y = %f", dy);/*
+ pc.printf("\t %d", p_actual);
+ pc.printf("\t %d", p_actual2);
+ pc.printf("\t %d", p_pom);
+ pc.printf("\t %d\r\n", p_pom2);*/
+ pc.printf("\t %d", p1);
+ pc.printf("\t %d \r\n", p2);
+
+
+
+ p1 = p_pom - p_actual;
+ p2 = p_pom2 - p_actual2;
- 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