RaC2018
added serial up, down, left and right control
Dependencies: mbed
Fork of
Robot_a_cables_v3_6
by 
RaC2018
Diff: main.cpp
- Revision:
- 10:fb3542f3c5e6
- Parent:
- 9:0f63d4cb5613
- Child:
- 11:0fa23c2dab76
--- a/main.cpp Fri May 18 09:03:25 2018 +0000
+++ b/main.cpp Mon May 28 14:18:32 2018 +0000
@@ -1,5 +1,6 @@
#include "mbed.h"
#include "parameters.h"
+#include "rac.h"
/*
Projet BTS SN - Robot à câbles - Lycée Georges Cabanis
Enzo Niro - Erwin Sazio
@@ -10,15 +11,13 @@
#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 MOD_DEG(p25);
DigitalOut MOD_NOR(p26);
+DigitalOut CYCLE_TIME(p20);
DigitalIn SWH(p5);
DigitalIn SWV(p6);
DigitalIn BPO(p7);
@@ -26,16 +25,16 @@
AnalogIn joystick_x(p16); // Pin 4 (fil jaune)
AnalogIn joystick_y(p17); // Pin 5 (fil bleu)
+RAC robot;
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;
+bool mode = 0; // 0 = Degraded 1 = Normal
int x,y,ctr_x,ctr_y; // Variables de joystick
uint8_t ping = 0; //variable pour etat de processus
uint8_t done = 0;
char command[8]; //word to send command
-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
@@ -46,7 +45,7 @@
//l1 = longueur moteur gauche à l'effecteur
//l2 = longueur moteur droite à l'effecteur
int32_t lx, l1, l2;
-float pVx, pVy; // variables pour afficher les vélocitées appliquées sur le moniteur
+double pVx, pVy; // variables pour afficher les vélocitées appliquées sur le moniteur
@@ -65,28 +64,7 @@
}
}
-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] );
- }// c'ets la valeur retournée par la fonction write
- else
- { emis_ok = 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);
- //lcd.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] );
- /*if(emis_ok) {
- // ici octet emis n'a pas de sens car trame remote !
- //pc.printf("send \r \n");
- } */
-}
+
void initialisation()
{
@@ -110,36 +88,35 @@
MOD_NOR = 0;
wait(0.1);
- command[0] = DRIVER_R1;
+ command[0] = DRIVER_R1;
command[1] = DRIVER_R2;
while(ping == 0) // INIT MOTEUR DROIT
{
pc.printf("ping droite ...\r\n");
- send(INITOP, command, 'D', 2); //send word to put pluto driver in operational mode
+ robot.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);
- }
+ 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);
+ }
- ping =0;
+ ping = 0;
command[0] = DRIVER_L1;
command[1] = DRIVER_L2;
while(ping == 0) // INIT MOTEUR GAUCHE
{
pc.printf("ping gauche ...\r\n");
- send(INITOP, command, 'D', 2); //send word to put pluto driver in operational mode
+ robot.send(INITOP, command, 'D', 2); //send word to put pluto driver in operational mode
MOD_DEG = 1;
if(can2.read(msg)) {
for(int i = 0; i < msg.len; i++){
@@ -155,118 +132,6 @@
} // fin initialisation
-
-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
- if(can2.read(msg)){
- 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];
- }
- //timer_process_read = 1;
- wait(0.005);
- send(TPDO1_R, command, 'R', 0); //Ask position for first 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];
- }
-
-
- }//fin de fonction
-
-
-
-void control_Moteur()
-{
- ///////////// CONTROLE MOTEUR //////////////////////////////////////////////////////////////////////////////
- // LECTURE Joystick + stockage valeurs dans x, y
- ctr_x = int(-((joystick_ctr.read()*100)-50));
- ctr_y = int((joystick_ctr.read()*100)-50);
- if(SWH){x = int(-((joystick_x.read()*100)-50))+ctr_x;}else{x=0;} // Detection x avec correction ctr de -42 à +42
- 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_R;
-
- if(mode ==0)
- {
- ///////////// MODE DEGRADE /////////////////////////////////////
- VX = (float(x)/42)*425000; // Calcule brut vélocité x, y
- VY = (float(y)/42)*425000;
- VM_L = VX - VY; // Calcule des vélocités exactes de chaques moteurs
- VM_R = -VX - VY;
-
- command[0]= (VM_L & 0x000000FF); // Masque + décalage pour translation big vers little endian
- command[1]= (VM_L & 0x0000FF00)>>8;
- command[2]= (VM_L & 0x00FF0000)>>16;
- command[3]= (VM_L & 0xFF000000)>>24;
- send(RPDO1_L, command, 'D', 4); // Controle moteur gauche
-
- command[0]= (VM_R & 0x000000FF);
- command[1]= (VM_R & 0x0000FF00)>>8;
- command[2]= (VM_R & 0x00FF0000)>>16;
- command[3]= (VM_R & 0xFF000000)>>24;
- send(RPDO1_R, command, 'D', 4); // Controle moteur droit
- ////////////////////////////////////////////////////////////////
- }
- else if(mode ==1)
- {
- ///////////// MODE NORMAL //////////////////////////////////////
- VX = x;
- VY = y;
- VM_L = ((dx*VX)-(dy*VY));
- VM_L = (VM_L/l1);
- VM_L = VM_L*(ALPHA/42);
- VM_R = ((-(lx-dx)*VX)-(dy*VY));
- VM_R = (VM_R/l2);
- VM_R = VM_R*(ALPHA/42);
-
- command[0]= (VM_L & 0x000000FF); // Masque + décalage pour translation big vers little endian
- command[1]= (VM_L & 0x0000FF00)>>8;
- command[2]= (VM_L & 0x00FF0000)>>16;
- command[3]= (VM_L & 0xFF000000)>>24;
- send(RPDO1_L, command, 'D', 4); // Controle moteur gauche
-
- command[0]= (VM_R & 0x000000FF);
- command[1]= (VM_R & 0x0000FF00)>>8;
- command[2]= (VM_R & 0x00FF0000)>>16;
- command[3]= (VM_R & 0xFF000000)>>24;
- send(RPDO1_R, command, 'D', 4); // Controle moteur droit
- }
- pVx = VM_R;
- pVy = VM_L;
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////
-}
int main() {
@@ -274,27 +139,37 @@
SWH.mode(PullUp);
SWV.mode(PullUp);
BPO.mode(PullUp);
-
+
initialisation(); // Mise en mode opérationnel des moteurs
-
-
-
while(1)
{
-
+ /*
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 VM_L = %f", pVx);
- pc.printf("\t VM_R = %f \r\n", pVy);
-
+ pc.printf("\t L2 = %d", l2);*/
+ if(mode == DEGRAD){
+ pc.printf("mode = D");
+ }
+ else if(mode == NORMAL){
+ pc.printf("mode = N");
+ }
+ pc.printf(" x = %f", dx);
+ pc.printf(" y = %f", dy);
+ pc.printf(" VM_L = %f", pVx);
+ pc.printf(" VM_R = %f \r\n", pVy);
- read_position();
- control_Moteur(); // Controle du meteur mode degradé et normal
+
+
+ robot.readButtons(SWH, SWV, BPO); //read state buttons, important for others functions
+
+ CYCLE_TIME = 0;
+ robot.readPosition(&dx, &dy, &mode); //read motors encoders and calculate coordinates position (x,y)
+ robot.setVelocity(joystick_x.read(),joystick_y.read(),joystick_ctr.read(), mode); //Motors control with the joystick and with the mode we use
+ robot.readVelocity(&pVx, &pVy);//read velocities applied on the robot
display_mode(); // Controle des led affichant le mode
+
+ CYCLE_TIME = 1;
}// fin while(1)