[code]
// include libraries
#include <mcp_can.h>
#include <mcp_can_dfs.h>
//---------------------------
//
// Variable initialisation
//
//---------------------------
#define VAR_TURN 1350
#define NEUTRAL 40
#define ACT_VAR 500
#define MAX_COMMAND 220 //90% of 255
#define MIN_COMMAND 30 //10% of 255
#define MAX_SPEED 2500
#define TIME_CAN 2
#define TIME_PROCESS 50
#define CURRENT_SAMPLE 10
#define MAX_CURRENT 400
#define VAR_CORRECT 1
#define VAR_SLOPE 10
#define SPI_CS_CAN 9
#define CORRECT_LEVER_R -150 // correction of the lever position
#define CORRECT_LEVER_L -80 // correction of the lever position
#define Codid_Epos_R 0x602 // CAN address of the EPOS right
#define Codid_Epos_L 0x603 // CAN address of the EPOS left
#define codid_rec_r 0x582 // CAN address for answer of the EPOS right
#define codid_rec_l 0x583 // CAN address for answer of the EPOS left
int current_r[CURRENT_SAMPLE];
int current_l[CURRENT_SAMPLE];
int speed_r = 0;
int speed_l = 0;
int pot_r = 0;
int pwr_r = 128;
int var_r = 0;
int correct_var_r = 0;
int pot_l = 0;
int pwr_l = 128;
int var_l = 0;
int correct_var_l = 0;
int port_pot_r = A0; //A0 input pin for pot_r
int port_pot_l = A1; //A1 input pin for pot_l
int port_kelly_ana_r = 5; //D5 output pin for kelly ana r
int port_kelly_ana_l = 6; //D6 output pin for kelly ana l
//int port_kelly_rev_r = 4; //D4 output pin for kelly rev r
int port_kelly_rev_l = 7; //D7 output pin for kelly rev l
int port_kelly_pwr = 4; //D12 output pin for kelly pwr
int port_switch_seat = 3; //D3 input pin for seat switch
int port_switch_brake = 2; //D2 input pin for brake switch
bool seat = 0; // state of the seat
bool brake = 0; // state of the break
bool stop_r = 1; // EPOS right is disable
bool stop_l = 1; // EPOS left is disable
bool start_secure = 1; //Security to avoid starting outside the neutral area
bool homing = 0;
bool dir_r = 0;
bool dir_l = 0;
bool message_kelly_r = 0; // status of the reception from kelly controler
bool message_kelly_l = 0; // status of the reception from kelly controler
unsigned char len = 0;
byte buf[8];
unsigned char canframe[8] = {0, 0, 0, 0, 0, 0, 0, 0};
unsigned long canId = 0;
unsigned char data_low_right = 0;
unsigned char data_low_left = 0;
unsigned char status_right = 0;
unsigned char status_left = 0;
unsigned char error_r = 0;
unsigned char error_l = 0;
unsigned int connection_test = 0;
MCP_CAN CAN(SPI_CS_CAN); // Set CS pin
void setup() {
// put your setup code here, to run once:
//Port Initialisation
pinMode(port_pot_r, INPUT);
pinMode(port_pot_l, INPUT);
pinMode(port_switch_seat, INPUT_PULLUP);
pinMode(port_switch_brake, INPUT_PULLUP);
pinMode(port_kelly_ana_r, OUTPUT);
pinMode(port_kelly_rev_l, OUTPUT);
pinMode(port_kelly_pwr, OUTPUT);
digitalWrite(port_kelly_pwr, LOW); //Disable the kellys
// send command to the motors
analogWrite(port_kelly_ana_r, pwr_r);
analogWrite(port_kelly_ana_l, pwr_l);
//init of the CAN
Serial.begin(115200);
while (CAN_OK != CAN.begin(CAN_250KBPS)) // init can bus : baudrate = 125k
{
Serial.println(“CAN BUS Shield init fail”);
Serial.println(" Init CAN BUS Shield again");
delay(100);
}
Serial.println(“CAN BUS Shield init ok!”);
// set mask, set both the mask to 0x3ff
CAN.init_Mask(0, 0, 0x7FF); // Mask 0 for filter 0 & 1 allows all ID from 0 to 7
CAN.init_Mask(1, 1, 0x1FFFFFFE); // mask 1 for filter 2 - 4
// set filter, we can receive id from 0x04 ~ 0x09
CAN.init_Filt(0, 0, 0x582); // there are 6 filter in mcp2515
CAN.init_Filt(1, 0, 0x583); // there are 6 filter in mcp2515
CAN.init_Filt(2, 1, 0x00); // there are 6 filter in mcp2515
CAN.init_Filt(3, 1, 0x00); // there are 6 filter in mcp2515
}
void loop() {
// put your main code here, to run repeatedly:
//--------------------------
//
// Checking Epos
//
//--------------------------
// check the status of the EPOS2 right (read statusword at 0x6041 subindex 0x00)
canframe[0] = 0x40;
canframe[1] = 0x41;
canframe[2] = 0x60;
canframe[3] = 0x00;
canframe[4] = 0x00;
canframe[5] = 0x00;
canframe[6] = 0x00;
canframe[7] = 0x00;
CAN.sendMsgBuf(Codid_Epos_R, 0, 4, canframe);
do {
delay(TIME_CAN);
connection_test++;
if (connection_test > 10) { //resend command every 10 cycles
delay(1000);
CAN.sendMsgBuf(Codid_Epos_R, 0, 4, canframe);
connection_test = 0;
Serial.println(“EPOS RIGHT NOT CONECTED … \r”);
}
} while (CAN_MSGAVAIL != CAN.checkReceive());
connection_test = 0;
CAN.readMsgBuf(&len, buf); // read data, len: data length, buf: data buf
data_low_right = buf[4];
if (data_low_right == 0 ) {
Serial.println(“EPOS RIGHT NOT CONECTED … \r”);
}
// check the status of the EPOS2 left (read statusword at 0x6041 subindex 0x00)
CAN.sendMsgBuf(Codid_Epos_L, 0, 4, canframe);
do {
delay(TIME_CAN);
} while (CAN_MSGAVAIL != CAN.checkReceive());
CAN.readMsgBuf(&len, buf); // read data, len: data length, buf: data buf
data_low_left = buf[4];
if (data_low_left == 0 ) {
Serial.println(“EPOS LEFT NOT CONECTED … \r”);
}
canframe[0] = 0x2F;
canframe[1] = 0x60;
canframe[2] = 0x60;
canframe[3] = 0x00;
canframe[4] = 0xFF;
canframe[5] = 0x00;
canframe[6] = 0x00;
canframe[7] = 0x00;
CAN.sendMsgBuf(Codid_Epos_R, 0, 5, canframe);
do {
delay(TIME_CAN);
} while (CAN_MSGAVAIL != CAN.checkReceive());
CAN.readMsgBuf(&len, buf); // read data, len: data length, buf: data buf
CAN.sendMsgBuf(Codid_Epos_L, 0, 5, canframe);
do {
delay(TIME_CAN);
} while (CAN_MSGAVAIL != CAN.checkReceive());
CAN.readMsgBuf(&len, buf); // read data, len: data length, buf: data buf
canframe[0] = 0x40;
canframe[1] = 0x01;
canframe[2] = 0x10;
canframe[3] = 0x00;
canframe[4] = 0x00;
canframe[5] = 0x00;
canframe[6] = 0x00;
canframe[7] = 0x00;
CAN.sendMsgBuf(Codid_Epos_R, 0, 4, canframe);
do {
delay(TIME_CAN);
} while (CAN_MSGAVAIL != CAN.checkReceive());
CAN.readMsgBuf(&len, buf); // read data, len: data length, buf: data buf
error_r = buf[4];
CAN.sendMsgBuf(Codid_Epos_L, 0, 4, canframe);
do {
delay(TIME_CAN);
} while (CAN_MSGAVAIL != CAN.checkReceive());
CAN.readMsgBuf(&len, buf); // read data, len: data length, buf: data buf
error_l = buf[4];
digitalWrite(port_kelly_pwr, HIGH); //enable the kellys
//reset status of kellys message
message_kelly_r = 0;
message_kelly_l = 0;
//read speed and current of the motor on kellys controllers
CAN.init_Filt(2, 1, 0x0CF11E11); // set filter for right kelly
delay (50);
while (!message_kelly_r || !message_kelly_l) {
do {
delay(TIME_CAN);
connection_test++;
if (connection_test > 1000) {
// digitalWrite(port_kelly_pwr, LOW);
connection_test = 0;
Serial.println("Message Bloqued");
}
} while (CAN_MSGAVAIL != CAN.checkReceive());
CAN.readMsgBuf(&len, buf); // read data, len: data length, buf: data buf
connection_test = 0;
canId = CAN.getCanId();
switch (canId) {
case (0x0CF11E10):
speed_r = buf[0] + 256 * buf[1];
current_r[0] = buf[2] + 256 * buf[3];
message_kelly_r = 1;
Serial.println("message R OK... \r");
break;
case (0x0CF11E11):
speed_l = buf[0] + 256 * buf[1];
current_l[0] = buf[2] + 256 * buf[3];
message_kelly_l = 1;
Serial.println("message L OK ... \r");
break;
}
}
// disable message from kellys
CAN.init_Filt(2, 1, 0x0000000); // there are 6 filter in mcp2515
}
[/code]