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
main.cpp
- Committer:
- protongamer
- Date:
- 2018-05-14
- Revision:
- 7:e87feff62bfd
- Parent:
- 6:ffffa9dfadfc
- Child:
- 8:1e6d3d9ae1d3
File content as of revision 7:e87feff62bfd:
#include "mbed.h"
#include "parameters.h"
/*
Projet BTS SN - Robot à câbles - Lycée Georges Cabanis
Enzo Niro - Erwin Sazio
Transmission par Bus CAN(protocol CANOpen)
*/
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);
DigitalIn SWH(p5);
DigitalIn SWV(p6);
DigitalIn BPO(p7);
AnalogIn joystick_ctr(p15); // Pin 2 (fil vert) Center Tap Reference
AnalogIn joystick_x(p16); // Pin 4 (fil jaune)
AnalogIn joystick_y(p17); // Pin 5 (fil bleu)
CAN can2(p30, p29, 1000000); // on definit pin et debit
CANMessage msg;
Serial pc(USBTX, USBRX);
uint8_t mode = 0;
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
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
int32_t lx, l1, l2;
void display_mode()
{
if(mode == 0) // MODE DEGRADE
{
MOD_DEG = 1;
MOD_NOR = 0;
}
else if(mode == 1) // MODE NORMAL
{
MOD_DEG = 0;
MOD_NOR = 1;
}
}
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()
{
pc.printf("starting...\r\n");
MOD_DEG = 1;
MOD_NOR = 0;
wait(0.1);
MOD_DEG = 0;
MOD_NOR = 1;
wait(0.1);
MOD_DEG = 1;
MOD_NOR = 0;
wait(0.1);
MOD_DEG = 0;
MOD_NOR = 1;
wait(0.1);
MOD_DEG = 1;
MOD_NOR = 0;
wait(0.1);
MOD_DEG = 0;
MOD_NOR = 0;
wait(0.1);
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
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);
}
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
MOD_DEG = 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_DEG = 0;
wait(0.9);
}
} // fin initialisation
void read_position(){
}//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
if(mode ==0)
{
///////////// MODE DEGRADE /////////////////////////////////////
float VX = (float(x)/42)*425000; // Calcule brut vélocité x, y
float VY = (float(y)/42)*425000;
long int VM_L = VX - VY; // Calcule des vélocités exactes de chaques moteurs
long int 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
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
}
int main() {
//define Pullup switch
SWH.mode(PullUp);
SWV.mode(PullUp);
BPO.mode(PullUp);
initialisation(); // Mise en mode opérationnel des moteurs
lx = LENGTH_MOTOR;
while(1)
{
timer_process_read++;
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){
timer_process_read = 0;
p_pom = p_actual;
p_pom2 = p_actual2;
wait(1.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];
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 = 0;
}
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;
//read_position();
control_Moteur(); // Controle du meteur mode degradé et normal
display_mode(); // Controle des led affichant le mode
}// fin while(1)
} // fin main