Enzo Niro
/
Robot_a_cables_v2_5
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
Robot_a_cables_v2_0
by 
RaC2018
Revision 7:e87feff62bfd, committed 2018-05-14
- Comitter:
- protongamer
- Date:
- Mon May 14 15:27:10 2018 +0000
- Parent:
- 6:ffffa9dfadfc
- Commit message:
- version 2.5;
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
| parameters.h | Show annotated file Show diff for this revision Revisions of this file |
--- 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
--- a/parameters.h Mon May 07 12:12:34 2018 +0000 +++ b/parameters.h Mon May 14 15:27:10 2018 +0000 @@ -98,5 +98,8 @@ //Length from motors to cursor in millimeters(mm) when the origin is taken //Left motor to cursor -#define LENGTH_L1_PO 1355 -#define LENGTH_L2_PO 1355 +#define LENGTH_L1_PO 1470 +//Right motor to cursor +#define LENGTH_L2_PO 1470 +//Left motor to Right motor +#define LENGTH_MOTOR 985