Robacab
/
Robot_a_cables_v3_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.
Dependencies: mbed
Revision 6:ffffa9dfadfc, committed 2018-05-07
- Comitter:
- protongamer
- Date:
- Mon May 07 12:12:34 2018 +0000
- Parent:
- 5:d5a021bbe81b
- Child:
- 7:e87feff62bfd
- Commit message:
- v2.1
Changed in this revision
--- a/can_parameters.h Mon Apr 30 12:27:56 2018 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,88 +0,0 @@ -/* -Here is the list of the CAN Codes to sent to the motor driver -There identifiers and word code -These parameters can be changed - -Here is the default values - - -//Identifier Codes - -#define INITOP 0x00 -#define RPDO1_R 0x215 -#define RPDO2_R 0x315 -#define RPDO3_R 0x415 -#define RPDO4_R 0x515 -#define TPDO1_R 0x195 -#define TPDO2_R 0x295 -#define TPDO3_R 0x395 -#define TPDO4_R 0x495 -#define RPDO1_L 0x220 -#define RPDO2_L 0x320 -#define RPDO3_L 0x420 -#define RPDO4_L 0x520 -#define TPDO1_L 0x1A0 -#define TPDO2_L 0x2A0 -#define TPDO3_L 0x3A0 -#define TPDO4_L 0x4A0 - - - -//Word codes - -//Driver adresses -#define DRIVER11 0x01 -#define DRIVER12 0x15 -#define DRIVER21 0x01 -#define DRIVER22 0x20 - - - -*/ - - - - - -//Identifier Codes - -/* -To set right value for: -RPDO1 => 0x200 + ID -RPDO2 => 0x300 + ID -RPDO3 => 0x400 + ID -RPDO4 => 0x500 + ID -TPDO1 => 0x180 + ID -TPDO2 => 0x280 + ID -TPDO3 => 0x380 + ID -TPDO4 => 0x480 + ID -*/ - - -#define INITOP 0x00 -#define RPDO1_R 0x215 -#define RPDO2_R 0x315 -#define RPDO3_R 0x415 -#define RPDO4_R 0x515 -#define TPDO1_R 0x195 -#define TPDO2_R 0x295 -#define TPDO3_R 0x395 -#define TPDO4_R 0x495 -#define RPDO1_L 0x220 -#define RPDO2_L 0x320 -#define RPDO3_L 0x420 -#define RPDO4_L 0x520 -#define TPDO1_L 0x1A0 -#define TPDO2_L 0x2A0 -#define TPDO3_L 0x3A0 -#define TPDO4_L 0x4A0 - - - -//Word codes - -//Driver adresses -#define DRIVER_R1 0x01 -#define DRIVER_R2 0x15 -#define DRIVER_L1 0x01 -#define DRIVER_L2 0x20
--- a/main.cpp Mon Apr 30 12:27:56 2018 +0000
+++ b/main.cpp Mon May 07 12:12:34 2018 +0000
@@ -1,5 +1,5 @@
#include "mbed.h"
-#include "can_parameters.h"
+#include "parameters.h"
/*
Projet BTS SN - Robot à câbles - Lycée Georges Cabanis
Enzo Niro - Erwin Sazio
@@ -32,10 +32,20 @@
uint8_t done = 0;
char command[8]; //word to send command
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;
+float 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
+
+//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;
+
+
+
+
void display_mode()
{
@@ -96,7 +106,7 @@
MOD_NOR = 0;
wait(0.1);
- command[0] = DRIVER_R1;
+ command[0] = DRIVER_R1;
command[1] = DRIVER_R2;
while(ping == 0) // INIT MOTEUR DROIT
@@ -140,6 +150,34 @@
}
} // fin initialisation
+
+
+void read_position(){
+
+
+
+
+
+ /* 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
+
+
+
void control_Moteur()
{
///////////// CONTROLE MOTEUR //////////////////////////////////////////////////////////////////////////////
@@ -186,32 +224,30 @@
initialisation(); // Mise en mode opérationnel des moteurs
- 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);
+ l1 = (LENGTH_L1_PO*1000) - (p1*1000)/2500; //Longueur 1
+
+ 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);
+ }
- dy = 525000 + (d_new*1000)/2500;
-
+ //dy = 525000 + (d_new*1000)/2500;
+
//wait_ms(1);
@@ -247,32 +283,34 @@
}
}*/
-
+ 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
+ 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;
- }*/
+ p_actual = msg.data[3];
+ p_actual = p_actual << 8;
+ p_actual = p_actual | msg.data[2];
+ p_actual = p_actual << 8;
+ p_actual = p_actual | msg.data[1];
+ p_actual = p_actual << 8;
+ p_actual = p_actual | msg.data[0];
+
}
+
+ p1 = p_pom - p_actual;
if(can2.read(msg)) {
for(int i = 0; i < msg.len; i++){
- pc.printf("0x%x ",msg.data[i]);
+ //pc.printf("0x%x ",msg.data[i]);
}//end of for
- pc.printf("\r \n");
+ //pc.printf("\r \n");
}//can.read(msg)
+ //read_position();
control_Moteur(); // Controle du meteur mode degradé et normal
display_mode(); // Controle des led affichant le mode
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/parameters.h Mon May 07 12:12:34 2018 +0000 @@ -0,0 +1,102 @@ +/* +Here is the list of the CAN Codes to sent to the motor driver +There identifiers and word code +These parameters can be changed + +Here is the default values + + +//Identifier Codes + +#define INITOP 0x00 +#define RPDO1_R 0x215 +#define RPDO2_R 0x315 +#define RPDO3_R 0x415 +#define RPDO4_R 0x515 +#define TPDO1_R 0x195 +#define TPDO2_R 0x295 +#define TPDO3_R 0x395 +#define TPDO4_R 0x495 +#define RPDO1_L 0x220 +#define RPDO2_L 0x320 +#define RPDO3_L 0x420 +#define RPDO4_L 0x520 +#define TPDO1_L 0x1A0 +#define TPDO2_L 0x2A0 +#define TPDO3_L 0x3A0 +#define TPDO4_L 0x4A0 + + + +//Word codes + +//Driver adresses +#define DRIVER11 0x01 +#define DRIVER12 0x15 +#define DRIVER21 0x01 +#define DRIVER22 0x20 + + + +*/ + + + + + +//Identifier Codes + +/* +To set right value for: +RPDO1 => 0x200 + ID +RPDO2 => 0x300 + ID +RPDO3 => 0x400 + ID +RPDO4 => 0x500 + ID +TPDO1 => 0x180 + ID +TPDO2 => 0x280 + ID +TPDO3 => 0x380 + ID +TPDO4 => 0x480 + ID +*/ + + +#define INITOP 0x00 +#define RPDO1_R 0x215 +#define RPDO2_R 0x315 +#define RPDO3_R 0x415 +#define RPDO4_R 0x515 +#define TPDO1_R 0x195 +#define TPDO2_R 0x295 +#define TPDO3_R 0x395 +#define TPDO4_R 0x495 +#define RPDO1_L 0x220 +#define RPDO2_L 0x320 +#define RPDO3_L 0x420 +#define RPDO4_L 0x520 +#define TPDO1_L 0x1A0 +#define TPDO2_L 0x2A0 +#define TPDO3_L 0x3A0 +#define TPDO4_L 0x4A0 + + + +//Word codes + +//Driver adresses +#define DRIVER_R1 0x01 +#define DRIVER_R2 0x15 +#define DRIVER_L1 0x01 +#define DRIVER_L2 0x20 + + + + + +//Robot setup +/* +Here is the configuration of the robot +*/ + +//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