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
main.cpp
- Committer:
- protongamer
- Date:
- 2018-04-23
- Revision:
- 4:3fd7c53d31c1
- Parent:
- 3:86e21a36eecb
- Child:
- 5:d5a021bbe81b
File content as of revision 4:3fd7c53d31c1:
#include "mbed.h"
#include "can_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(p26);
DigitalOut MOD_NOR(p25);
DigitalIn SWH(p5);
DigitalIn SWV(p6);
DigitalIn BPO(p7);
CAN can1(p9, p10,1000000);// on definit pin et debit
CAN can2(p30, p29, 1000000);
Serial pc(USBTX, USBRX);
//AnalogIn pot_1(p19); // potard pour selection
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");
} */
}
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;
int main() {
CANMessage msg;
uint8_t ping = 0; //variable pour etat de processus
uint8_t done = 0;
char command[8]; //word to send command
//define Pullup switch
SWH.mode(PullUp);
SWV.mode(PullUp);
BPO.mode(PullUp);
pc.printf("starting...\r\n");
MOD_DEG = 1;
wait(0.1);
MOD_DEG = 0;
wait(0.1);
MOD_DEG = 1;
wait(0.1);
MOD_DEG = 0;
wait(0.1);
MOD_DEG = 1;
wait(0.1);
MOD_DEG = 0;
wait(0.1);
command[0] = DRIVER_R1;
command[1] = DRIVER_R2;
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);
}
MOD_NOR = 1;
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);
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);
}
}
}*/
if(timer_process_read >= 10){
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;
}*/
}
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)
switch(BPO){
case 0:
if(done == 0){
timer_process_read = 50;
//timer_process_ping = 0;
switch(SWV){
case 0:
command[0] = 0x00;
command[1] = 0x8F;
command[2] = 0x00;
command[3] = 0x00;
send(RPDO1_R, command, 'D', 4); //send word to put pluto driver in operational mode
break;
case 1:
command[0] = 0xFF;
command[1] = 0x70;
command[2] = 0xFF;
command[3] = 0xFF;
send(RPDO1_R, command, 'D', 4); //send word to put pluto driver in operational mode
break;
}//end of switch SWV
done = 1;
}
break;
case 1:
if(done == 1){
command[0] = 0x00;
command[1] = 0x00;
command[2] = 0x00;
command[3] = 0x00;
send(RPDO1_R, command, 'D', 4); //send word to put pluto driver in operational mode
done = 0;
//send(TPDO1_R, command, 'R', 0); //send word to put pluto driver in operational mode
}
break;
}
}// fin while(1)
} // fin main