RaC2018
/
Robot_a_cables_v3_0
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
Fork of
Robot_a_cables_v2_6_5
by 
RaC2018
Revision 10:e4e1228604fb, committed 2018-05-23
- Comitter:
- protongamer
- Date:
- Wed May 23 12:33:04 2018 +0000
- Parent:
- 9:0f63d4cb5613
- Commit message:
- 42
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 Fri May 18 09:03:25 2018 +0000
+++ b/main.cpp Wed May 23 12:33:04 2018 +0000
@@ -10,15 +10,14 @@
#define DEGRAD 0
#define NORMAL 1
- //#define ALPHA 425000
- #define ALPHA 425000
+
+
-DigitalOut MOTOR1_OP(LED1);
-DigitalOut MOTOR2_OP(LED2);
-DigitalOut DEB_MOD_DEG(LED3);
-DigitalOut DEB_MOD_NOR(LED4);
+DigitalOut MOTOR_L_COM(LED1);
+DigitalOut MOTOR_R_COM(LED3);
DigitalOut MOD_DEG(p25);
DigitalOut MOD_NOR(p26);
+DigitalOut CYCLE_TIME(p20);
DigitalIn SWH(p5);
DigitalIn SWV(p6);
DigitalIn BPO(p7);
@@ -28,7 +27,7 @@
CAN can2(p30, p29, 1000000); // on definit pin et debit
CANMessage msg;
-Serial pc(USBTX, USBRX);
+Serial pc(USBTX, USBRX, 115200);
uint8_t mode = 0;
int x,y,ctr_x,ctr_y; // Variables de joystick
@@ -67,17 +66,15 @@
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 )) ;
+ { 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] );
}// c'ets la valeur retournée par la fonction write
else
- { emis_ok = can2.write(CANMessage(id, octet_emis, longueur, CANRemote, CANStandard ));
+ { can2.write(CANMessage(id, octet_emis, longueur, CANRemote, 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] );
}
//lcd.locate(0,10);
@@ -110,7 +107,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
@@ -120,19 +117,19 @@
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);
- }
+ for(int i = 0; i < msg.len; i++){
+ pc.printf("0x%x ",msg.data[i]);
+ ping = 1;
+ MOTOR_R_COM = 1;
+ }//end of for
+ pc.printf("\r \n");
+ }//can.read(msg)
+ wait(0.1);
+ MOD_NOR = 0;
+ wait(0.9);
+ }
- ping =0;
+ ping = 0;
command[0] = DRIVER_L1;
command[1] = DRIVER_L2;
@@ -145,6 +142,7 @@
for(int i = 0; i < msg.len; i++){
pc.printf("0x%x ",msg.data[i]);
ping = 1;
+ MOTOR_L_COM = 1;
}//end of for
pc.printf("\r \n");
}//can.read(msg)
@@ -157,24 +155,6 @@
void read_position(){
-
- //timer_process_read++;//var used to set time for reading value
- l1 = LENGTH_L1_PO - (p1/2545); //Length 1
- l2 = LENGTH_L2_PO - (p2/2545); //Length 2
- lx = LENGTH_MOTOR;
- dx = (((double) l1*(double) l1) - ((double) l2*(double) l2) + ((double) lx*(double) lx))/(2*(double) lx);
- dy = sqrt(((double) l1*(double) l1) - (dx*dx));
-
- if(BPO == 0){
-
- p_pom = p_actual;
- p_pom2 = p_actual2;
- mode = !mode; //Change mode
- wait(1.0);
- }
-
- p1 = p_pom - p_actual;
- p2 = p_pom2 - p_actual2;
//Left motor
send(TPDO1_L, command, 'R', 0); //Ask position for second length
@@ -186,9 +166,10 @@
p_actual = p_actual | msg.data[1];
p_actual = p_actual << 8;
p_actual = p_actual | msg.data[0];
+ MOTOR_L_COM = 1;
}
//timer_process_read = 1;
- wait(0.005);
+ wait(0.01);
send(TPDO1_R, command, 'R', 0); //Ask position for first length
if(can2.read(msg)){
p_actual2 = msg.data[3];
@@ -198,8 +179,28 @@
p_actual2 = p_actual2 | msg.data[1];
p_actual2 = p_actual2 << 8;
p_actual2 = p_actual2 | msg.data[0];
+ MOTOR_R_COM = 1;
}
-
+
+
+ if(BPO == 0){
+ p_pom = p_actual; //Refresh first origin value
+ p_pom2 = p_actual2; //Refresh second origin value
+ mode = !mode; //Change mode
+ wait(1.0);
+ }
+
+ //Calculate positions values in counters
+ p1 = p_pom - p_actual;
+ p2 = p_pom2 - p_actual2;
+
+
+ //Calculate Coordinates
+ l1 = LENGTH_L1_PO - (p1/REFERENCE); //Length 1
+ l2 = LENGTH_L2_PO - (p2/REFERENCE); //Length 2
+ lx = LENGTH_MOTOR;
+ dx = (((double) l1*(double) l1) - ((double) l2*(double) l2) + ((double) lx*(double) lx))/(2*(double) lx);
+ dy = sqrt(((double) l1*(double) l1) - (dx*dx));
}//fin de fonction
@@ -215,7 +216,7 @@
if(SWV){y = int((joystick_y.read()*100)-50)+ctr_y;}else{y=0;} // Detection y avec correction ctr
float VX; // Calcule brut vélocité x, y
float VY;
- long int VM_L; // Calcule des vélocités exactes de chaques moteurs
+ long int VM_L;
long int VM_R;
if(mode ==0)
@@ -244,9 +245,11 @@
///////////// MODE NORMAL //////////////////////////////////////
VX = x;
VY = y;
+ //Left control
VM_L = ((dx*VX)-(dy*VY));
VM_L = (VM_L/l1);
VM_L = VM_L*(ALPHA/42);
+ //Right control
VM_R = ((-(lx-dx)*VX)-(dy*VY));
VM_R = (VM_R/l2);
VM_R = VM_R*(ALPHA/42);
@@ -276,9 +279,6 @@
BPO.mode(PullUp);
initialisation(); // Mise en mode opérationnel des moteurs
-
-
-
while(1)
{
@@ -291,10 +291,14 @@
pc.printf("\t VM_L = %f", pVx);
pc.printf("\t VM_R = %f \r\n", pVy);
-
+ CYCLE_TIME = 0;
+ MOTOR_L_COM = 0;
+ MOTOR_R_COM = 0;
read_position();
control_Moteur(); // Controle du meteur mode degradé et normal
display_mode(); // Controle des led affichant le mode
+
+ CYCLE_TIME = 1;
}// fin while(1)
--- a/parameters.h Fri May 18 09:03:25 2018 +0000 +++ b/parameters.h Wed May 23 12:33:04 2018 +0000 @@ -1,49 +1,14 @@ -/* -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 +//Parameters of the robot +//includes CANopen codes and robot parameters -//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 - - - +/* +Here is the list of the CANopen Codes to sent to the motor driver +There identifiers and word code +These parameters can be changed */ - - - //Identifier Codes /* @@ -79,9 +44,11 @@ -//Word codes + -//Driver adresses +//Driver adresses for operational mode +//Example : DRIVER_R1 = 0x01 DRIVER_R2= 0x15 ----> Code must be sent = DRIVER_R1 + DRIVER_R2 = 0x0115 + #define DRIVER_R1 0x01 #define DRIVER_R2 0x15 #define DRIVER_L1 0x01 @@ -92,14 +59,97 @@ //Robot setup -/* -Here is the configuration of the robot -*/ + +//Here is the configuration of the robot +//These value can be changed + //Length from motors to cursor in millimeters(mm) when the origin is taken + //Left motor to cursor #define LENGTH_L1_PO 781 + //Right motor to cursor #define LENGTH_L2_PO 781 + //Left motor to Right motor #define LENGTH_MOTOR 1000 + +//Max velocity for normal mode +#define ALPHA 425000 + +//Number of counters for 1 millimeters +#define REFERENCE 2545 + + + + +/*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 + + +//Driver adresses for operational mode +//Example : DRIVER_R1 = 0x01 DRIVER_R2= 0x15 ----> Code must be sent = DRIVER_R1 + DRIVER_R2 = 0x0115 + +#define DRIVER11 0x01 +#define DRIVER12 0x15 +#define DRIVER21 0x01 +#define DRIVER22 0x20 + + + +//Robot setup + +//Here is the configuration of the robot +//These value can be changed + + +//Length from motors to cursor in millimeters(mm) when the origin is taken + +//Left motor to cursor +#define LENGTH_L1_PO 781 + +//Right motor to cursor +#define LENGTH_L2_PO 781 + +//Left motor to Right motor +#define LENGTH_MOTOR 1000 + +//Max velocity for normal mode +#define ALPHA 425000 + +//Number of counters for 1 millimeters +#define REFERENCE 2545 + + + + +*/ + + + + + + + +