void debug_output_string(const char *v)
{
t_can_simple_var_packet p;
p.type = SIMPLE_VAR_TYPE_STRING;
p.ctr = debug_output_next_ctr++;
strncpy(p.data, v, 6);
ecan_send(SIMPLE_VAR_OUTPUT_ID, (unsigned char *)&p, 8, 0);
}
// simple var output protocol
void debug_output_flush()
{
t_can_simple_var_packet p;
p.type = SIMPLE_VAR_TYPE_FLUSH;
p.ctr = debug_output_next_ctr;
debug_output_next_ctr = 0;
ecan_send(SIMPLE_VAR_OUTPUT_ID, (unsigned char *)&p, 8, 0);
}
void debug_output_int32(const char *var_name, int32_t v)
{
t_can_simple_var_packet p;
debug_output_string(var_name);
p.type = SIMPLE_VAR_TYPE_INT32;
p.ctr = debug_output_next_ctr++;
memcpy(p.data, &v, sizeof(v));
ecan_send(SIMPLE_VAR_OUTPUT_ID, (unsigned char *)&p, 8, 0);
}
// Main Interrupt Service Routine (ISR)
void interrupt ISR(void)
{
if(PIR3bits.RXB0IF)
{
Message newMessage;
ecan_receive_rxb0(&newMessage);
ecan_rxb0_clear(); // Clear flag
newMessage.sid = 0x123;
ecan_send(&newMessage);
PIR3bits.RXB0IF = 0;
}
if(PIR3bits.RXB1IF)
{
Message newMessage;
ecan_receive_rxb1(&newMessage);
ecan_rxb1_clear();
newMessage.sid = 0x123;
ecan_send(&newMessage);
PIR3bits.RXB1IF = 0;
}
}
void canstdio_send(const char *data, int data_len)
{
int chunk_start, i;
// invia dati in messaggi CAN di 8 byte ciascuno
// non fare nulla se l'output non è stato attivato
for (chunk_start = 0; chunk_start < data_len && nodo_attivo; chunk_start += 7)
{
int chunk_len = data_len - chunk_start;
if (chunk_len > 6) chunk_len = 6;
t_can_remote_stdio_endpoint2coord m;
m.seqnum = seqnum + 1;
m.data_len = chunk_len | (id_nodo_locale << 4);
memcpy(m.data, data + chunk_start, chunk_len);
// Attesa massima clear-to-send: circa 1 secondo
for (i = 0; i < 10000 && clear_to_send == false; i++)
__delay_us(100);
if (clear_to_send == false)
{
nodo_attivo = false;
break;
}
ecan_send(REMOTE_STDIO_CAN_ID(REMOTE_STDIO_COORD_ID), (void*)&m, 8, 0);
// Attesa massima: 1 secondo
for (i = 0; i < 10000 && seqnum != m.seqnum; i++)
__delay_us(100);
if (seqnum != m.seqnum) // Niente ACK entro il timeout, assumiamo che il coordinatore sia andato giù e ci disattiviamo
{
nodo_attivo = false;
break;
}
}
}
void main(void)
{
/* Initialization functions called below */
ConfigureOscillator();
uart_init(9600);
ecan_init();
enable_interrupts();
/* While doing recurring functions */
while(1)
{
/* Check if UART contains data*/
if(uart_data_ready())
{
Message mess;
char num = uart_read();
if (num == 'i')
{
mess.sid = 0x00A;
mess.len = 1;
uart_write_text("Command Sent (Array Idle)\r\n");
mess.data[0] = 0x00;
ecan_send(&mess);
}
else if (num == 'f')
{
mess.sid = 0x00A;
mess.len = 1;
uart_write_text("Command Sent (Array Forward)\r\n");
mess.data[0] = 0x01;
ecan_send(&mess);
}
else if (num == 'b')
{
mess.sid = 0x00A;
mess.len = 1;
uart_write_text("Command Sent (Array Backward)\r\n");
mess.data[0] = 0x02;
ecan_send(&mess);
}
else if (num == '0')
{
mess.sid = 0x007;
mess.len = 1;
uart_write_text("Command Sent (Motor Speed 0)\r\n");
mess.data[0] = 0x00;
ecan_send(&mess);
}
else if (num == '1')
{
mess.sid = 0x007;
mess.len = 1;
uart_write_text("Command Sent (Motor Speed 1)\r\n");
mess.data[0] = 0x01;
ecan_send(&mess);
}
else if (num == '2')
{
mess.sid = 0x007;
mess.len = 1;
uart_write_text("Command Sent (Motor Speed 2)\r\n");
mess.data[0] = 0x02;
ecan_send(&mess);
}
else if (num == '3')
{
mess.sid = 0x007;
mess.len = 1;
uart_write_text("Command Sent (Motor Speed 3)\r\n");
mess.data[0] = 0x03;
ecan_send(&mess);
}
else if (num == '4')
{
mess.sid = 0x007;
mess.len = 1;
uart_write_text("Command Sent (Motor Speed 4)\r\n");
mess.data[0] = 0x04;
ecan_send(&mess);
}
else if (num == '5')
{
mess.sid = 0x007;
mess.len = 1;
uart_write_text("Command Sent (Motor Speed 5)\r\n");
mess.data[0] = 0x05;
ecan_send(&mess);
}
else if (num == '6')
{
mess.sid = 0x007;
mess.len = 1;
uart_write_text("Command Sent (Motor Speed 6)\r\n");
mess.data[0] = 0x06;
ecan_send(&mess);
}
else if (num == '7')
{
mess.sid = 0x007;
mess.len = 1;
uart_write_text("Command Sent (Motor Speed 7)\r\n");
mess.data[0] = 0x07;
ecan_send(&mess);
}
else if (num == '8')
{
mess.sid = 0x007;
mess.len = 1;
uart_write_text("Command Sent (Motor Speed 8)\r\n");
mess.data[0] = 0x08;
ecan_send(&mess);
}
else if (num == '9')
{
mess.sid = 0x007;
mess.len = 1;
uart_write_text("Command Sent (Motor Speed 9)\r\n");
mess.data[0] = 0x09;
ecan_send(&mess);
}
else if (num == '-')
{
mess.sid = 0x007;
mess.len = 1;
uart_write_text("Command Sent (Motor Speed 10)\r\n");
mess.data[0] = 0x0A;
ecan_send(&mess);
}
else if (num == '!') //! brake!
{
mess.sid = 0x00B;
mess.len = 1;
uart_write_text("Command Sent (Brake On)\r\n");
mess.data[0] = 0x01;
ecan_send(&mess);
}
else if (num == 'g') //go - brake off
{
mess.sid = 0x00B;
mess.len = 1;
uart_write_text("Command Sent (Brake Off)\r\n");
mess.data[0] = 0x00;
ecan_send(&mess);
}
}
}
}
void main(void)
{
/* Initialization functions called below */
ecan_init();
ConfigureOscillator();
/* Enable Interrupts */
RCONbits.IPEN = 0; // Enable interrupt priority
INTCONbits.GIE = 1; // Enable interrupts
INTCONbits.PEIE = 1; // Enable peripheral interrupts.
PIE3bits.RXB0IE = 1; // Enable CAN receive buffer 0 interrupt
PIE3bits.RXB1IE = 1; // Enable CAN receive buffer 1 interrupt
i2c_init();
//Configure accelerometer CONTRL_REG1_XM
//char slaveAddr0 = 0x1E;
//char destAddr0 = 0x20;
//char sendData0 = 0b01100111; //== 0x67
//i2c_writeToReg(slaveAddr0, destAddr0, sendData0);
//CONTROL_REG5_XM
//slaveAddr0 = 0x1E;
//destAddr0 = 0x24;
//sendData0 = 0b11110000;
//i2c_writeToReg(slaveAddr0, destAddr0, sendData0);
i2c_writeToReg(0x1E, 0x1F, 0x00);
i2c_writeToReg(0x1E, 0x20, 0x57);
i2c_writeToReg(0x1E, 0x21, 0x00);
i2c_writeToReg(0x1E, 0x22, 0x00);
i2c_writeToReg(0x1E, 0x23, 0x00);
i2c_writeToReg(0x1E, 0x24, 0x14);
i2c_writeToReg(0x1E, 0x25, 0x00);
i2c_writeToReg(0x1E, 0x26, 0x00);
i2c_writeToReg(0x6A, 0x20, 0x0F);
i2c_writeToReg(0x6A, 0x21, 0x00);
i2c_writeToReg(0x6A, 0x23, 0x00);
i2c_writeToReg(0x6A, 0x24, 0x00);
/* While doing recurring functions */
while(1)
{
/*
short ac1 = i2c_get_data(0x77, 0xAA);
short ac2 = i2c_get_data(0x77, 0xAC);
short ac3 = i2c_get_data(0x77, 0xAE);
unsigned short ac4 = i2c_get_udata(0x77, 0xB0);
unsigned short ac5 = i2c_get_udata(0x77, 0xB2);
unsigned short ac6 = i2c_get_udata(0x77, 0xB4);
short b1 = i2c_get_data(0x77, 0xB6);
short b2 = i2c_get_data(0x77, 0xB8);
short mb = i2c_get_data(0x77, 0xBA);
short mc = i2c_get_data(0x77, 0xBC);
short md = i2c_get_data(0x77, 0xBF);
i2c_write_raw(0xF4, 0x2E);
__delay_ms(5);
long ut = i2c_get_data(0x77, 0xF6);
long x1 = ((long) ut - (unsigned long) ac6) * (unsigned long) ac5 / 32768;
long x2 = (long) mc * 2048 / (x1 + (long) md);
long b5 = x1 + x2;
long temp = (b5 + 8) / 16;
Message tempPack;
tempPack.sid = 0x101;
tempPack.len = 4;
tempPack.data[3] = temp >> 24;
tempPack.data[2] = temp >> 16;
tempPack.data[1] = temp >> 8;
tempPack.data[0] = temp;
ecan_send(&tempPack);
__delay_ms(50);
*/
/*short xGyroRaw = i2c_get_byte(0x6A, 0x28);
xGyroRaw += i2c_get_byte(0x6A,0x29) << 8;
short yGyroRaw = i2c_get_byte(0x6A, 0x2A);
yGyroRaw += i2c_get_byte(0x6A,0x2B) << 8;
short zGyroRaw = i2c_get_byte(0x6A, 0x2C);
zGyroRaw += i2c_get_byte(0x6A,0x2D) << 8;
Message gyroPack;
gyroPack.sid = 0x000;
gyroPack.len = 6;
gyroPack.data[5] = xGyroRaw >> 8;
gyroPack.data[4] = xGyroRaw;
gyroPack.data[3] = yGyroRaw >> 8;
gyroPack.data[2] = yGyroRaw;
gyroPack.data[1] = zGyroRaw >> 8;
gyroPack.data[0] = zGyroRaw;
ecan_send(&gyroPack); */
short xAccelRaw = i2c_get_byte(0x1E, 0x28);
xAccelRaw += i2c_get_byte(0x1E,0x29) << 8;
short yAccelRaw = i2c_get_byte(0x1E, 0x2A);
yAccelRaw += i2c_get_byte(0x1E,0x2B) << 8;
short zAccelRaw = i2c_get_byte(0x1E, 0x2C);
zAccelRaw += i2c_get_byte(0x1E,0x2D) << 8;
Message accelPack;
accelPack.sid = 0x000;
accelPack.len = 6;
accelPack.data[5] = xAccelRaw >> 8;
accelPack.data[4] = xAccelRaw;
accelPack.data[3] = yAccelRaw >> 8;
accelPack.data[2] = yAccelRaw;
accelPack.data[1] = zAccelRaw >> 8;
accelPack.data[0] = zAccelRaw;
ecan_send(&accelPack);
}
}
