char handshake()
{
uart_flush();
_delay_ms(100);
while(uart_available()==0)
{
PORTB |=(1<<PB5);
_delay_ms(100);
PORTB &=~(1<<PB5);
_delay_ms(100);
}
if (uart_getc() == 'a')
{
PORTB |=(1<<PB5);
_delay_ms(50);
PORTB &=~(1<<PB5);
_delay_ms(50);
PORTB |=(1<<PB5);
_delay_ms(50);
PORTB &=~(1<<PB5);
_delay_ms(50);
}
uart_putc('b');
while(uart_available()==0)
{
PORTB |=(1<<PB5);
_delay_ms(20);
PORTB &= ~(1<<PB5);
_delay_ms(20);
}
if (uart_getc() == 'c')
{
uart_putc('d');
PortMode = REGULAR;
return(0); // handshake success
}
else
{
uart_puts_p(PSTR("\n Debug mode"));
PortMode = DEBUG;
return(1); // debug mode
}
}
void command_usart_input(uint8_t event, char *cmd) {
uint8_t status, current;
if (event == 0) {
if (uart_available()) { // data ready
current = uart_get();
status = command_usart_parse(¤t);
if (status == CHAR_NORMAL) {
if (g_buffer_pos >= B_SIZE) { // reset when full
g_buffer_pos = 0;
}
uart_put(current); // echo valid char back
g_buffer[g_buffer_pos] = current; // add to buffer
g_command_status = CMD_IN_PROCESS;
g_buffer_pos++;
} else if (status == CHAR_ENTER) { // enter pressed
g_buffer[g_buffer_pos] = '\0'; // mark current pos as end
g_buffer_pos = 0;
g_command_status = CMD_RDY;
}
}
if (g_command_status == CMD_RDY) { // copy buffer to command
strcpy(cmd, g_buffer);
g_command_status = CMD_IN_PROCESS;
} else { // set command as "still empty"
cmd[0] = '\0';
}
}
}
char key_get_key(void)
{
uart_tx('C'); //tell the keyboard to send the last key
while(!uart_available()) {}
return uart_read_buff();
}
void process_message(void) {
uint8_t message_type;
if (uart_available() < MESSAGE1_LENGTH ) {
_delay_ms(1000); // So we have a full message to process
}
if (uart_available() < MESSAGE1_LENGTH ) {
print("Something went wrong, only have partial message!\n");
}
// First character should indicate message type
message_type = uart_getchar();
if (message_type != '!') {
print("Received unrecognized Message type!\n");
}
if (message_type == '!') {
fill_in_message1();
}
return;
}
int main(void) {
uart_init(BAUD_RATE);
outlets_init();
messages_index = 0;
replied_messages_index = 0;
uint8_t c;
while (1) {
/* Echo code for testing
if (uart_available()) {
c = uart_getchar();
print("Byte: ");
pchar(c);
pchar('\n');
}
*/
if (uart_available()) {
process_message();
}
if (messages_index != replied_messages_index) {
print_message1(messages[replied_messages_index]);
// Check product key match, strncmp returns 0 if equal
if(!strncmp(messages[replied_messages_index].board_product_key, "1234511111", MESSAGE1_PRODUCT_KEY_LENGTH)) {
int outlet_number = atoi_length(messages[replied_messages_index].board_outlet_number, MESSAGE1_OUTLET_NUMBER_LENGTH);
if (outlet_number < OUTLET_COUNT) {
int outlet_state = atoi_length(messages[replied_messages_index].state, MESSAGE1_STATE_LENGTH);
if(outlet_state == 1 || outlet_state == 0) {
outlet_states[outlet_number] = outlet_state;
// Send message to imp
send_message2();
} else {
//print("Bad outlet state\n");
}
} else {
//print("Outlet number greater than number of outlets!\n");
}
} else {
//print("Message for a different board\n");
}
replied_messages_index++;
if(replied_messages_index >= MESSAGES_QUEUE_LENGTH) {
replied_messages_index = 0;
}
}
set_outlets_state();
}
}
void main()
{
// Buffer para almacenar teclas
uint8_t buf[10];
uint8_t index = 0;
ANSEL = 0x00;
TRISA = 0xFF;
TRISB = 0b00000100;
// Get UART ID, The UART library works on parts with more than one UART/USART modules
xUARTHandle uart1 = uart_init(E_UART_1);
// Configure UART (standard serial port settings)
if (uart_control(uart1, UART_DATABITS_8 | UART_PARITY_NONE | UART_STOPBITS_1, 9600))
for (;;); // Invalid UART settings, should not get here
// Open UART and begin usage
if (uart_open(uart1)) {
// Send a string
uart_puts(uart1, "PIC UART Demo\r\n");
// Main loop
for (;;) {
// Check if UART received some data
if (uart_available(uart1)) {
// receive a single byte
buf[index] = uart_read(uart1);
// send a single byte (echo)
uart_write(uart1, buf[index]);
// Send a string with control characters
uart_puts(uart1, "\r\n");
// Increase counter and check limits
index++;
if (index == 10) {
// Send other string
uart_puts(uart1, "\r\nRX: ");
// Send the 10 characters array
uart_write_array(uart1, buf, 10);
uart_puts(uart1, "\r\n");
// Reset index
index = 0;
}
}
}
}
for (;;); // Should not get here
}
int main(void)
{
uint8_t cmd_pos;
uint8_t valid;
CPU_PRESCALE(0); // run at 16 MHz
uart_init(BAUD_RATE);
init_lpd(32);
lpd_show();
colorChase(Color(0,0,127), 10);
while (1)
{
if (uart_available())
{
uint8_t c = uart_getchar();
if (c=='M')
{
cmd_pos = 0;
valid = 0;
}
else if ((c >= '0') && (c <= '9'))
{
setPixelColor(cmd_pos, get_color(c));
cmd_pos++;
}
else if (c == '!')
{
valid = 1;
}
else
{
cmd_pos = 0;
}
}
if (valid)
{
lpd_show();
valid = 0;
}
}
}
// A very basic example...
// when the user types a character, print it back
int main(void)
{
uint8_t signals[3] = {0, 0, 0};
CPU_PRESCALE(0); // run at 16 MHz
pwm_init();
uart_init(BAUD_RATE);
set_signals(signals);
while (1) {
if (uart_available()) {
uart_get_pwms(signals);
set_signals(signals);
}
}
}
static int8_t main_state_machine() {
static int8_t state = STATE_POST;
static uint8_t skippy = 16;
if (--skippy == 0) {
lastReceiverStatus = spdif_readStatus();
skippy = 8;
}
switch (state) {
case STATE_READY:
if (spdif_queryStatusIRQ()) {
spdif_clearIRQ();
//state = SPDIF_IRQ;
break;
}
if (uart_available()) {
state = STATE_DBGINPUT;
debug_getinput(DBGINPUT_INIT);
}
break;
case STATE_DBGINPUT:
if (debug_getinput(-1) == DBGINPUT_WAIT) {
state = STATE_RESPOND;
}
break;
case STATE_RESPOND:
debug_respond();
state = STATE_POST;
break;
case SPDIF_IRQ:
//dummy8 = spdif_readStatus();
//decodeAndPrintStatus();
state = STATE_POST;
break;
case STATE_POST:
printf("\n# ");
state = STATE_READY;
break;
}
return state;
}
int main(void)
{
initLED();
initDOTS();
uart_init(UART_BAUD_SELECT(38400, F_CPU));
sei();
uart_flush();
_delay_ms(100);
uint8_t i=0;
char c;
memset(to_write, 0x00, sizeof(to_write));
PORTD &=~RESETPIN;
_delay_ms(1);
PORTD |= RESETPIN;
for (uint8_t i=0; i<200; i++) {
writeCol(0xFF);
}
for (;;) {
while(uart_available() == 0) {
PORTB|=(1<<PB5);
_delay_ms(100);
PORTB&=~(1<<PB5);
_delay_ms(100);
}
c = uart_getc();
char c_lo = c;
uart_putc(c);
if (c_lo >= 0x20) {
to_write[i++] = c_lo;
}
if ((c_lo < 0x20) || (i >= 12)) {
writeArray(to_write);
i=0;
}
}
return 0; /* never reached */
}
int8_t debug_getinput(int8_t initState) {
static int8_t state = 0;
static uint8_t bufidx;
uint8_t input;
if (initState != -1) {
state = initState;
}
switch (state) {
case DBGINPUT_INIT:
bufidx = 0;
state = DBGINPUT_INPUT;
break;
case DBGINPUT_INPUT:
if (uart_available()) {
if (bufidx < BUF_SIZE - 1) {
inputBuffer[bufidx] = input = uart_getc();
if (input == '\r') {
state = DBGINPUT_DONE;
} else {
(void)putchar(input);
bufidx++;
}
} else {
state = DBGINPUT_DONE;
}
}
break;
case DBGINPUT_DONE:
inputBuffer[++bufidx] = 0;
printf("\n");
state = DBGINPUT_WAIT;
break;
case DBGINPUT_WAIT:
// endstate
break;
}
return state;
}
/** @brief Main function for UART example / test program.
*
* @return None (never returns).
*
* Sits in a loop waiting for characters on the given UART, then sends
* a string telling what it got.
*/
int main()
{
/* Choose the UART to use for test */
UART_Type *uart = UART;
uint8_t c;
/* Set up uart for 8n2 */
init_uart(uart, BAUD);
uart_putstr(uart, "Now echoing characters: ");
/* Loop forever, reporting what is received */
while(1) {
if (uart_available(uart)) {
c = uart_getchar(uart);
uart_putchar(uart, c);
}
}
}
uint8_t readline(char* buffer, uint8_t max_len)
{
uint16_t c;
uint8_t size = 0;
while(1)
{
c = uart_getc();
if(!uart_available(c))
continue;
if ((char) c == '\n')
{
buffer[size] = '\0';
return size;
}
buffer[size] = (char)c;
size++;
if (size > max_len)
{
uart_puts_P("ERROR: Line to long\n");
return 0;
}
}
}
uint8_t HardwareSerial::available(void)
{
return uart_available(_uart);
}
void check_drive_uart(void) {
static uint8_t inputbuf[RX_LINE_SIZE], inputptr = 0;
uint8_t i, recv;
while(uart_available(DRIVE)) {
recv = uart_get(DRIVE);
if(recv == '\r') {
if(inputptr) {
if(inputbuf[0] == '@') {
// reverse command from DRIVE MCU
switch(inputbuf[1]) {
case '1': // drive complete
if(inputptr != 3 || inputbuf[2] != '0') { break; }
drive_complete = 1;
break;
case '4': // track info update
switch(inputbuf[2]) {
case '4': // absolute position
if(inputptr != 11) { break; }
abspL = htoa(inputbuf[ 3], inputbuf[ 4]) << 8 | htoa(inputbuf[ 5], inputbuf[ 6]);
abspR = htoa(inputbuf[ 7], inputbuf[ 8]) << 8 | htoa(inputbuf[ 9], inputbuf[10]);
break;
case '5': // relative position
if(inputptr != 15) { break; }
sectL = htoa(inputbuf[ 3], inputbuf[ 4]);
relpL = htoa(inputbuf[ 5], inputbuf[ 6]) << 8 | htoa(inputbuf[ 7], inputbuf[ 8]);
sectL = htoa(inputbuf[ 9], inputbuf[10]);
relpL = htoa(inputbuf[11], inputbuf[12]) << 8 | htoa(inputbuf[13], inputbuf[14]);
break;
case '6': // track sensors
if(inputptr != 7) { break; }
trksL = htoa(inputbuf[3], inputbuf[4]);
trksR = htoa(inputbuf[5], inputbuf[6]);
break;
}
break;
}
}
else if(rev_passthru) {
for(i = 0; i < inputptr; i++) { uart_put(DEBUG, inputbuf[i]); }
fprintf(&debug, "\r\n");
}
inputptr = 0;
}
}
else if(recv == '\n') {
;
}
else {
if(inputptr != RX_LINE_SIZE) {
inputbuf[inputptr] = recv;
inputptr++;
}
}
}
}
char getMessage(SerMsg *msg,uint16_t *data)
{
static uint8_t alt=0;
uint8_t raw_string[10];
uint8_t cln_string[10];
static uint16_t yaw =512;
static uint16_t pitch=512;
switch(PortMode)
{
case DEBUG:
uart_puts_P("\r\n wasd >");
while(uart_available()==0);
char c = uart_getc();
switch (c)
{
case 'a':
yaw+=50;
*msg = YAWPOS;
*data = yaw;
break;
case 'd':
yaw-=50;
*msg = YAWPOS;
*data = yaw;
break;
case 'w':
pitch -=50;
*msg = PITCHPOS;
*data = pitch;
break;
case 's':
pitch +=50;
*msg = PITCHPOS;
*data = pitch;
break;
}
return 0;
break;
case REGULAR:
if (uart_available()>0)
{
uint8_t i =0;
raw_string[0] = uart_getc();
while (raw_string[i] != 0x7F && uart_available())
{
raw_string[i++] = uart_getc(); // get to new frame
}
raw_string[i]=uart_getc();
if (raw_string[i]==0x7F)
{
raw_string[i] = uart_getc();
}
while (uart_available())
{
raw_string[++i] = uart_getc();
if (raw_string[i]==0x7F)
{break;}
}
clean_string(cln_string,raw_string);
switch(cln_string[0])
{
case 'p':
if (cln_string[1]=='p')
{
*msg = YAWPOS;
*data = (cln_string[2] << 8) + cln_string[3];
return 0; //success!
}
else
{
return -1;
}
break;
case 'o':
if( cln_string[1]=='o')
{
*msg = PITCHPOS;
*data = (cln_string[2] << 8) + cln_string[3];
return 0; //success!
} else
{
return -1;
}
break;
case 's':
*msg = STOP;
*data = 0;
return 0;
break;
}
}
else
{
switch(alt){
case 0:
uart_putc('o');
alt =1;
break;
case 1:
uart_putc('p');
alt=0;
break;
default:
uart_putc('p');
alt = 0;
break;
}
}
break;
default:
*msg = NULL;
return -1; // error.
}
return 0;
}
int main()
{
init();
_delay_ms(100);
debug("uart port test\r\n");
//GLCD.SetColor(0, 0);
GLCD.ClearScreen();
draw_logo();
GLCD.DrawString(30, 65, "System Booting...");
/*if (!ep)
{
GLCD.DrawString(30, 65, "read eeprom error");
}
else
{
char str[64];
sprintf(str, "%d bright %d ", sizeof(setting), setting.brightness);
GLCD.DrawString(30, 65, str);
}
*/
reset_2560();
//draw_image(0, 0, IMG_
//GLCD.DrawString(9, 40, "Mavlink Antenna Tracker");
//GLCD.DrawString(18, 50, "System Initiating...");
//link.send_message(DEV_READY, NULL, 0);
back_light_timeout = setting.el_timeout * 50;
while (1)
{
wdt_reset();
while (uart_available())
{
uint8_t c = uart_getc();
if (link.parse_message(c, &msg))
{
// got_msg = true;
on_recv_message();
}
wdt_reset();
}
if (tick)
{
tick = false;
//uart_putc('t');
debug("t");
link.timeout_tick();
Keyboard.Update();
uint8_t press = Keyboard.GetPressKey();
if (press)
{
debug("key pressed\r\n");
link.send_message(KEY_PRESS, &press, 1);
if (back_light_timeout == 0)
GLCD.TurnOnBackLight();
back_light_timeout = setting.el_timeout * 50;
}
if (setting.el_timeout > 0 && back_light_timeout > 0 && --back_light_timeout == 0)
GLCD.TurnOffBackLight();
}
}
return 0;
}
void check_debug_uart(void) {
static uint8_t inputbuf[RX_LINE_SIZE], inputptr = 0;
uint8_t i, recv;
int16_t ticks;
while(uart_available(DEBUG)) {
recv = uart_get(DEBUG);
if(recv == '\r') {
fprintf(&debug, "\r\n");
if(inputptr) {
switch(inputbuf[0]) {
case '?': // print drive command list
fprintf_P(&debug, PSTR("stop() ........................ | p00\r\n"));
fprintf_P(&debug, PSTR("fwd_both(speed) ............... | p0300, p0400, p15 u8\r\n"));
fprintf_P(&debug, PSTR("rev_both(speed) ............... | p0301, p0401, p15 u8\r\n"));
fprintf_P(&debug, PSTR("forward(Lspeed, Rspeed) ....... | p0300, p0400, p11 u8, p12 u8\r\n"));
fprintf_P(&debug, PSTR("reverse(Lspeed, Rspeed) ....... | p0301, p0401, p11 u8, p12 u8\r\n"));
fprintf_P(&debug, PSTR("turnCCW(Lspeed, Rspeed) ....... | p0301, p0400, p11 u8, p12 u8\r\n"));
fprintf_P(&debug, PSTR("turnCW (Lspeed, Rspeed) ....... | p0300, p0401, p11 u8, p12 u8\r\n"));
fprintf_P(&debug, PSTR("set_abs_pos(pos) .............. | p1a s16\r\n"));
fprintf_P(&debug, PSTR("set_rel_pos(sect, pos) ........ | p1b u8 u8\r\n"));
fprintf_P(&debug, PSTR("pos_corr_on() ................. | p1f01\r\n"));
fprintf_P(&debug, PSTR("pos_corr_off() ................ | p1f00\r\n"));
fprintf_P(&debug, PSTR("nav_abs_pos(speed, pos) ....... | p31 u8 s16\r\n"));
fprintf_P(&debug, PSTR("nav_rel_pos(speed, sect, pos) . | p32 u8 u8 u8\r\n"));
break;
case 'p': // passthrough to DRIVE MCU
for(i = 1; i < inputptr; i++) { uart_put(DRIVE, inputbuf[i]); }
uart_put(DRIVE, '\r');
break;
case 'r': // toggle reverse passthrough from DRIVE MCU
rev_passthru ^= 1;
break;
case 'd': // local dump on/off
local_dump ^= 1;
break;
case 's': // start/stop main thread
run_main ^= 1;
if(run_main) { fprintf_P(&debug, PSTR("Main thread started!\r\n")); }
else { fprintf_P(&debug, PSTR("Main thread stopped!\r\n")); stop(); }
break;
case 't': // start/stop test thread
if(inputptr != 2) { cmd_err(); break; }
run_test = htoa(0, inputbuf[1]);
if(!run_test) { fprintf_P(&debug, PSTR("All test sequences stopped!\r\n")); stop(); }
else { fprintf_P(&debug, PSTR("Started test sequence %u!\r\n"), run_test); }
break;
case ' ': // stop all motors
run_main = 0;
run_test = 0;
pid_on = 0;
stop();
set_speed_3(0);
set_speed_4(0);
break;
case 'u':
fprintf_P(&debug, PSTR("%lu\r\n"), uptime());
break;
case 'b':
#define VBAT_FACTOR 0.0044336
fprintf_P(&debug, PSTR("4 x %1.2fV\r\n"), (float)read_adc(VSENS) * VBAT_FACTOR);
break;
case 'm': // servo power
if(inputptr != 2 || (inputbuf[1] & ~1) != '0') { cmd_err(); break; }
inputbuf[1] == '0' ? clr_bit(SPWR) : set_bit(SPWR);
break;
case '9': // magnets
if(inputptr != 2 || (inputbuf[1] & ~1) != '0') { cmd_err(); break; }
if(inputbuf[1] == '0') { clr_bit(FET1); clr_bit(FET2); fprintf_P(&debug, PSTR("Magnets off!\r\n")); }
else { set_bit(FET1); set_bit(FET2); fprintf_P(&debug, PSTR("Magnets on!\r\n")); }
break;
case '1': // PID on/off
if(inputptr != 2 || (inputbuf[1] & ~1) != '0') { cmd_err(); break; }
if(inputbuf[1] == '0') { pid_on = 0; fprintf_P(&debug, PSTR("PID off!\r\n")); }
else { pid_on = 1; fprintf_P(&debug, PSTR("PID on!\r\n")); reset_pid(); }
break;
case '3': // turn motor commands
if(!isHex(inputbuf[2]) || !isHex(inputbuf[3])) { cmd_err(); break; }
switch(inputbuf[1]) {
case '0': // forward (uint8_t speed)
if(inputptr != 4) { cmd_err(); break; }
motor3_fwd();
set_speed_3(htoa(inputbuf[2], inputbuf[3]) * 40);
break;
case '1': // reverse (uint8_t speed)
if(inputptr != 4) { cmd_err(); break; }
motor3_rev();
set_speed_3(htoa(inputbuf[2], inputbuf[3]) * 40);
break;
case '2': // set reference target (int16_t ticks)
if(inputptr != 6 || !isHex(inputbuf[4]) || !isHex(inputbuf[5])) { cmd_err(); break; }
pid_target[MOTOR3] = htoa(inputbuf[2], inputbuf[3]) << 8 | htoa(inputbuf[4], inputbuf[5]);
break;
case '3': // set reference speed (int16_t ticks)
if(inputptr != 6 || !isHex(inputbuf[4]) || !isHex(inputbuf[5])) { cmd_err(); break; }
pid_speed[MOTOR3] = htoa(inputbuf[2], inputbuf[3]) << 8 | htoa(inputbuf[4], inputbuf[5]);
break;
case '4': // set P (int16_t factor)
if(inputptr != 6 || !isHex(inputbuf[4]) || !isHex(inputbuf[5])) { cmd_err(); break; }
//ENC3_P = htoa(inputbuf[2], inputbuf[3]) << 8 | htoa(inputbuf[4], inputbuf[5]);
break;
case '5': // set I (int16_t factor)
if(inputptr != 6 || !isHex(inputbuf[4]) || !isHex(inputbuf[5])) { cmd_err(); break; }
//ENC3_I = htoa(inputbuf[2], inputbuf[3]) << 8 | htoa(inputbuf[4], inputbuf[5]);
break;
case '6': // set D (int16_t factor)
if(inputptr != 6 || !isHex(inputbuf[4]) || !isHex(inputbuf[5])) { cmd_err(); break; }
//ENC3_D = htoa(inputbuf[2], inputbuf[3]) << 8 | htoa(inputbuf[4], inputbuf[5]);
break;
case '7': // set noise gate (int16_t level)
if(inputptr != 6 || !isHex(inputbuf[4]) || !isHex(inputbuf[5])) { cmd_err(); break; }
//ENC3_NOISE_GATE = htoa(inputbuf[2], inputbuf[3]) << 8 | htoa(inputbuf[4], inputbuf[5]);
break;
case 'f': // set reference angle
if(inputptr != 6 || !isHex(inputbuf[4]) || !isHex(inputbuf[5])) { cmd_err(); break; }
ticks = deg2ticks(htoa(inputbuf[2], inputbuf[3]) << 8 | htoa(inputbuf[4], inputbuf[5]));
cli();
V_encoder = ticks;
sei();
reset_pid();
break;
default:
cmd_err();
}
break;
case '4': // lift motor commands
if(!isHex(inputbuf[2]) || !isHex(inputbuf[3])) { cmd_err(); break; }
switch(inputbuf[1]) {
case '0': // up (uint8_t speed)
if(inputptr != 4) { cmd_err(); break; }
motor4_fwd();
set_speed_4(htoa(inputbuf[2], inputbuf[3]) * 40);
break;
case '1': // down (uint8_t speed)
if(inputptr != 4) { cmd_err(); break; }
motor4_rev();
set_speed_4(htoa(inputbuf[2], inputbuf[3]) * 40);
break;
case '2': // set reference target (int16_t ticks)
if(inputptr != 6 || !isHex(inputbuf[4]) || !isHex(inputbuf[5])) { cmd_err(); break; }
pid_target[MOTOR4] = htoa(inputbuf[2], inputbuf[3]) << 8 | htoa(inputbuf[4], inputbuf[5]);
break;
case '3': // set reference speed (int16_t ticks)
if(inputptr != 6 || !isHex(inputbuf[4]) || !isHex(inputbuf[5])) { cmd_err(); break; }
pid_speed[MOTOR4] = htoa(inputbuf[2], inputbuf[3]) << 8 | htoa(inputbuf[4], inputbuf[5]);
break;
case '4': // set P (int16_t factor)
if(inputptr != 6 || !isHex(inputbuf[4]) || !isHex(inputbuf[5])) { cmd_err(); break; }
//ACTU_P = htoa(inputbuf[2], inputbuf[3]) << 8 | htoa(inputbuf[4], inputbuf[5]);
break;
case '5': // set I (int16_t factor)
if(inputptr != 6 || !isHex(inputbuf[4]) || !isHex(inputbuf[5])) { cmd_err(); break; }
//ACTU_I = htoa(inputbuf[2], inputbuf[3]) << 8 | htoa(inputbuf[4], inputbuf[5]);
break;
case '6': // set D (int16_t factor)
if(inputptr != 6 || !isHex(inputbuf[4]) || !isHex(inputbuf[5])) { cmd_err(); break; }
//ACTU_D = htoa(inputbuf[2], inputbuf[3]) << 8 | htoa(inputbuf[4], inputbuf[5]);
break;
case '7': // set noise gate (int16_t level)
if(inputptr != 6 || !isHex(inputbuf[4]) || !isHex(inputbuf[5])) { cmd_err(); break; }
//ACTU_NOISE_GATE = htoa(inputbuf[2], inputbuf[3]) << 8 | htoa(inputbuf[4], inputbuf[5]);
break;
default:
cmd_err();
}
break;
case '5': // servo5 commands
if(inputptr != 4 || !isHex(inputbuf[2]) || !isHex(inputbuf[3])) { cmd_err(); break; }
servo5(htoa(inputbuf[2], inputbuf[3]));
//OCR0A = htoa(inputbuf[1], inputbuf[2]);
break;
case '6': // servo6 commands
if(inputptr != 4 || !isHex(inputbuf[2]) || !isHex(inputbuf[3])) { cmd_err(); break; }
servo6(htoa(inputbuf[2], inputbuf[3]));
//OCR0B = htoa(inputbuf[1], inputbuf[2]);
break;
case '7': // servo7 commands
if(inputptr != 4 || !isHex(inputbuf[2]) || !isHex(inputbuf[3])) { cmd_err(); break; }
servo7(htoa(inputbuf[2], inputbuf[3]));
//OCR2A = htoa(inputbuf[1], inputbuf[2]);
break;
case '8': // servo8 commands
if(inputptr != 4 || !isHex(inputbuf[2]) || !isHex(inputbuf[3])) { cmd_err(); break; }
servo8(htoa(inputbuf[2], inputbuf[3]));
//OCR2B = htoa(inputbuf[1], inputbuf[2]);
break;
case 'x':
if(!isHex(inputbuf[2]) || !isHex(inputbuf[3])) { cmd_err(); break; }
switch(inputbuf[1]) {
case '1': // set reference angle
if(inputptr != 6 || !isHex(inputbuf[4]) || !isHex(inputbuf[5])) { cmd_err(); break; }
param1 = htoa(inputbuf[2], inputbuf[3]) << 8 | htoa(inputbuf[4], inputbuf[5]);
break;
case '2': // set reference angle
if(inputptr != 6 || !isHex(inputbuf[4]) || !isHex(inputbuf[5])) { cmd_err(); break; }
param2 = htoa(inputbuf[2], inputbuf[3]) << 8 | htoa(inputbuf[4], inputbuf[5]);
break;
case '3': // set reference angle
if(inputptr != 6 || !isHex(inputbuf[4]) || !isHex(inputbuf[5])) { cmd_err(); break; }
param3 = htoa(inputbuf[2], inputbuf[3]) << 8 | htoa(inputbuf[4], inputbuf[5]);
break;
case '4': // set reference angle
if(inputptr != 6 || !isHex(inputbuf[4]) || !isHex(inputbuf[5])) { cmd_err(); break; }
param4 = htoa(inputbuf[2], inputbuf[3]) << 8 | htoa(inputbuf[4], inputbuf[5]);
break;
case '5': // set reference angle
if(inputptr != 6 || !isHex(inputbuf[4]) || !isHex(inputbuf[5])) { cmd_err(); break; }
param5 = htoa(inputbuf[2], inputbuf[3]) << 8 | htoa(inputbuf[4], inputbuf[5]);
break;
}
break;
default:
cmd_err();
}
inputptr = 0;
}
}
else if(recv == 0x7f) {
if(!inputptr) { uart_put(DEBUG, '\a'); }
else {
fprintf(&debug, "\b\e[K");
inputptr--;
}
}
else {
if(inputptr == RX_LINE_SIZE) { uart_put(DEBUG, '\a'); }
else {
uart_put(DEBUG, recv);
inputbuf[inputptr] = recv;
inputptr++;
}
}
}
void uart_flush()
{
uint8_t uart_tmp;
while(uart_available()) uart_tmp = UDR;
}
//--------------------------------------------------------------------------
// Transmit message over the bus. Message is readed from the given
// buffer position. While transmitting we check if bus is free and if
// same bit was recieved as transmitted. This ensures correct handling
// This method is time critical and interrupts will be disabled.
//--------------------------------------------------------------------------
posptr_t ibus_transmit_msg(posptr_t from, posptr_t len)
{
posptr_t oldFrom = from;
#ifdef RXTX_COMPARE
if (g_ibus_State != IBUS_STATE_TRANSMITTING) return from;
IBUS_RX_DIS_INT();
#else
if (IBUS_SENSTA_VALUE()) return from;
// we currently disable interrupt of TH3122, we will ask it manually here
IBUS_SENSTA_DIS_INT();
#endif
while(len > 0)
{
// load byte to transmit
uint8_t byte = g_ibus_TxBuffer[from];
uint8_t sent = byte;
uint8_t par = 0;
// transmit start bit and check if collision happens
bit_clear(IBUS_TX_PORT, IBUS_TX_PIN);
#ifdef RXTX_COMPARE
IBUS_BAUD_HDELAY();
if (IBUS_RX_VALUE() != 0) goto collision;
IBUS_BAUD_HDELAY();
#else
IBUS_BAUD_DELAY();
if (IBUS_SENSTA_VALUE()) goto collision;
#endif
// transmit bitwise and compute parity bit
// while transmitting check if collision was detected
int8_t i;
for (i = 0; i < 8; i++, byte>>=1)
{
par ^= (byte & 1);
if (byte & 1)
bit_set(IBUS_TX_PORT, IBUS_TX_PIN);
else
bit_clear(IBUS_TX_PORT, IBUS_TX_PIN);
#ifdef RXTX_COMPARE
IBUS_BAUD_HDELAY();
if (IBUS_RX_VALUE() != (byte & 1)) goto collision;
IBUS_BAUD_HDELAY();
#else
IBUS_BAUD_DELAY();
if (IBUS_SENSTA_VALUE()) goto collision;
#endif
}
// send parity bit
if (par & 1)
bit_set(IBUS_TX_PORT, IBUS_TX_PIN);
else
bit_clear(IBUS_TX_PORT, IBUS_TX_PIN);
#ifdef RXTX_COMPARE
IBUS_BAUD_HDELAY();
if (IBUS_RX_VALUE() != (par & 1)) goto collision;
IBUS_BAUD_HDELAY();
#else
IBUS_BAUD_DELAY();
if (IBUS_SENSTA_VALUE()) goto collision;
#endif
// send stop bit
bit_set(IBUS_TX_PORT, IBUS_TX_PIN);
#ifdef RXTX_COMPARE
IBUS_BAUD_HDELAY();
if (IBUS_RX_VALUE() != 1) goto collision;
IBUS_BAUD_HDELAY();
#else
IBUS_BAUD_DELAY();
if (IBUS_SENSTA_VALUE()) goto collision;
#endif
// update current pointers
from = inc_posptr_tx(from);
len--;
// we should have now received the same byte through uart rx
// check if there was no frame error and if the received byte is the right one
if (!uart_available()) goto collision;
if (uart_lastc() != sent) goto collision;
if (uart_last_error()) goto collision;
}
// if everything went fine, then we will return new from position
oldFrom = from;
// return on failure
collision:
IBUS_TX_SETUP();
#ifdef RXTX_COMPARE
IBUS_RX_ENA_INT();
#else
IBUS_SENSTA_ENA_INT();
#endif
//END_ATOMAR;
return oldFrom;
}
void decode_packet() {
unsigned short cs, ccs, newbits;
if (buf[1] == 1 && buf[2] == 'r') {
// reset from board
first = true;
PORTD |= LED_ALL;
return;
}
if (buf[1] != 16) {
goto bad_packet;
}
memcpy(&cs, &buf[16], 2);
ccs = calc_cksum((unsigned short *)buf, 8);
if (cs != ccs) {
goto bad_packet;
}
memcpy(targets, &buf[2], 12);
memcpy(&newbits, &buf[14], 2);
if (newbits != iobits) {
iobits = newbits;
PORTD |= (iobits & IOBIT_MASK) | LED_RED;
PORTD &= (iobits | ~IOBIT_MASK);
}
if (first) {
first = false;
memcpy(counts, targets, 12);
}
for (unsigned char i = 0; i != 6; ++i) {
short diff = (short)(targets[i] - counts[i]);
if (diff < -10000) {
diff = 10000;
}
else if (diff > 10000) {
diff = 10000;
}
else if (diff < 0) {
diff = -diff;
}
if (diff < 2) {
diff = 2;
}
steprates[i] = 25000 / diff;
}
// no longer blue, no longer lost packet
PORTD &= ~LED_BLUE;
blue_timeout = 0;
uart_send_all(3, ack_packet);
return;
bad_packet:
// bad packets make me feel blue
PORTD |= LED_BLUE;
wdt_reset();
unsigned short max = 0;
uart_send_all(3, nak_packet);
while (!uart_available()) {
udelay(10);
if (max++ == 20) {
// no byte is there
return;
}
}
// deliberately de-sync, because I might be treating
// data as sync bytes
uart_getch();
}
void main_loop(void *) {
toggle_debug();
if (uart_available()) {
PORTD |= LED_YELLOW;
unsigned char ch = (unsigned char)uart_getch();
if (recv_ptr == 0) {
if (ch != 0xed) {
// not a sync byte
}
else {
buf[0] = ch;
recv_ptr = 1;
}
}
else if (recv_ptr == 1) {
if (ch > sizeof(buf) - 2) {
// not a proper packet
recv_ptr = 0;
recv_end = 0;
}
else {
buf[1] = ch;
recv_end = 2 + ch;
recv_ptr = 2;
}
}
else {
buf[recv_ptr] = ch;
++recv_ptr;
if (recv_ptr == recv_end) {
decode_packet();
recv_ptr = 0;
recv_end = 0;
}
}
PORTD &= ~LED_YELLOW;
}
unsigned short now = uread_timer();
unsigned short delta = now - prev;
prev = now;
// if this loop takes 10 milliseconds, we're already in trouble...
if (delta > 10000) {
delta = 10000;
}
//uart_force_out(((unsigned char *)&delta)[0]);
//uart_force_out(((unsigned char *)&delta)[1]);
unsigned char mask = 1;
bool change = false;
for (unsigned char i = 0; i != 6; ++i) {
if (targets[i] != counts[i]) {
stepphases[i] += delta;
if ((steps & mask) || (stepphases[i] >= steprates[i])) {
change = true;
if (!(steps & mask)) {
stepphases[i] -= steprates[i];
}
// avoid too much accumulation of phase -- this
// means a limit on slew rate
if (stepphases[i] > 40000) {
stepphases[i] = 40000;
}
steps = steps ^ mask;
if ((short)(targets[i] - counts[i]) > 0) {
directions |= mask;
if (!(steps & mask)) {
counts[i]++;
}
}
else {
directions &= ~mask;
if (!(steps & mask)) {
counts[i]--;
}
}
}
}
mask = mask << 1;
}
DIR_PORT = directions;
if (change) {
PORTD |= LED_RED;
}
else {
PORTD &= ~LED_RED;
}
blue_timeout += delta;
if (blue_timeout > 2000000) {
PORTD |= LED_BLUE;
first = true;
}
after(0, main_loop, 0);
STEP_PORT = steps;
}
/**
* contains stuff borrowed from EasyTransfer lib
*/
uint8_t kspio_boardReceiveData() {
uint8_t i;
uint16_t c;
unsigned char chr;
if (kspio_rxLen == 0 && uart_available() > 3) {
c = uart_getc();
chr = (unsigned char) c;
while (chr != 0xBE) {
if (uart_available() == 0) {
return 0;
}
c = uart_getc(); // read character from UART
chr = (unsigned char) c; // low byte is the actual character, store that in chr
}
c = uart_getc();
chr = (unsigned char) c;
if (chr == 0xEF) {
c = uart_getc(); kspio_rxLen = (unsigned char) c;
c = uart_getc(); kspio_id = (unsigned char) c;
kspio_rxArrayInx = 1;
switch(kspio_id) {
case 0:
kspio_structSize = sizeof(kspio_hPacket);
kspio_address = (uint8_t*)&kspio_hPacket;
break;
case 1:
kspio_structSize = sizeof(kspio_vData);
kspio_address = (uint8_t*)&kspio_vData;
break;
}
}
//make sure the binary structs on both Arduinos are the same size.
if (kspio_rxLen != kspio_structSize) {
kspio_rxLen = 0;
return 0;
}
}
if (kspio_rxLen != 0) {
while (uart_available() && kspio_rxArrayInx <= kspio_rxLen) {
c = uart_getc();
kspio_buffer[kspio_rxArrayInx++] = (unsigned char) c;
}
kspio_buffer[0] = kspio_id;
if (kspio_rxLen == (kspio_rxArrayInx-1)) {
//seem to have got whole message
//last uint8_t is CS
kspio_calcCS = kspio_rxLen;
for (i = 0; i<kspio_rxLen; i++) {
kspio_calcCS^=kspio_buffer[i];
}
if (kspio_calcCS == kspio_buffer[kspio_rxArrayInx-1]) {//CS good
memcpy(kspio_address, kspio_buffer, kspio_structSize);
kspio_rxLen = 0;
kspio_rxArrayInx = 1;
return 1;
} else {
//failed checksum, need to clear this out anyway
kspio_rxLen = 0;
kspio_rxArrayInx = 1;
return 0;
}
}
}
return 0;
}
//--------------------------------------------------------------------------
void ibus_tick()
{
// if we have data in the receive buffer, then handle it
while(uart_available())
{
uint8_t data = uart_getc();
if (uart_last_error())
{
uart_clear_error();
g_ibus_State = IBUS_STATE_WAIT_FREE_BUS;
IBUS_TIMEOUT_RECEIVE_ERROR();
break;
}else
ibus_recieveCallback(data);
}
// if no data in the buffer or we are not idle, then do nothing
if (!ibus_readyToTransmit()) return;
// ok we are idle and we have data in the buffer, then first, wait for free buffer
#ifndef RXTX_COMPARE
if (IBUS_SENSTA_VALUE())
{
g_ibus_State = IBUS_STATE_WAIT_FREE_BUS;
IBUS_TIMEOUT_WAIT_FREE_BUS();
return;
}
#endif
g_ibus_State = IBUS_STATE_TRANSMITTING;
uart_clear_error(); // start fresh :)
// ok bus is free, we can submit a message
posptr_t tryCounterPos = g_ibus_TxReadPos;
int8_t numberOfTries = g_ibus_TxBuffer[g_ibus_TxReadPos]; g_ibus_TxReadPos = inc_posptr_tx(g_ibus_TxReadPos);
posptr_t len = (posptr_t)g_ibus_TxBuffer[(g_ibus_TxReadPos + 1) & IBUS_MSG_TX_BUFFER_SIZE_MASK];
// transmit message
posptr_t oldTxPos = g_ibus_TxReadPos;
posptr_t newTxPos = ibus_transmit_msg(g_ibus_TxReadPos, len + 2);
// from here we are free to clear all errors
uart_clear_error();
// if there was a collision, then resend message if we still have trials
if (newTxPos == oldTxPos)
{
numberOfTries--;
if (numberOfTries >= 0)
{
g_ibus_TxBuffer[tryCounterPos] = numberOfTries;
g_ibus_TxReadPos = tryCounterPos;
g_ibus_State = IBUS_STATE_WAIT_FREE_BUS;
IBUS_TIMEOUT_COLLISION();
return;
}
newTxPos = (posptr_t)(oldTxPos + len + 2) & IBUS_MSG_TX_BUFFER_SIZE_MASK;
// if there was no collision, then we have received the same message as sent, so just flush it
}else
uart_flush(0);
// ok message was either submitted successfully or message trials
// counter is elapsed, we flush tx buffer and continue
g_ibus_TxReadPos = newTxPos;
g_ibus_State = IBUS_STATE_WAIT_FREE_BUS;
IBUS_TIMEOUT_AFTER_TRANSMIT();
}
int USerial::available(void) {
return (uart_available());
}
void readPacket() {
// reset previous response
if ((_response_Available == true) || (_response_ERROR > 0)) {
// discard previous packet and start over
resetResponse();
}
while (uart_available()) {
b = uart_getc();
if (_pos > 0 && b == START_BYTE && ATAP == 2) {
// new packet start before previous packeted completed -- discard previous packet and start over
_response_ERROR = UNEXPECTED_START_BYTE;
uart_putc(_response_ERROR);
return;
}
if (_pos > 0 && b == ESCAPE) {
if (uart_available()) {
b = uart_getc();
b = 0x20 ^ b;
} else {
// escape byte. next byte will be
_escape = true;
continue;
}
}
if (_escape == true) {
b = 0x20 ^ b;
_escape = false;
}
// checksum includes all bytes starting with api id
if (_pos >= API_ID_INDEX) {
_checksumTotal+= b;
}
switch(_pos) {
case 0:
if (b == START_BYTE) {
_pos++;
}
break;
case 1:
// length msb
_response_MsbLength = b;
_pos++;
break;
case 2:
// length lsb
_response_LsbLength = b;
_pos++;
break;
case 3:
_response_ApiId = b;
_pos++;
break;
default:
// starts at fifth byte
if (_pos > MAX_FRAME_DATA_SIZE) {
// exceed max size. should never occur
_response_ERROR = PACKET_EXCEEDS_BYTE_ARRAY_LENGTH;
uart_putc(_response_ERROR);
return;
}
// check if we're at the end of the packet
// packet length does not include start, length, or checksum bytes, so add 3
if (_pos == (getPacketLength() + 3)) {
// verify checksum
if ((_checksumTotal & 0xff) == 0xff) {
_response_Checksum = b;
_response_Available = true;
//no check
//uart_puts_P("OK\n");///////////////////////////
//DDRA = 0x02;////////////////////////////////////
//PORTA = 0x02;//////////////////////////
_response_ERROR = NO_ERROR;
} else {
// checksum failed
_response_ERROR = CHECKSUM_FAILURE;
uart_putc(_response_ERROR);
}
// minus 4 because we start after start,msb,lsb,api and up to but not including checksum
// e.g. if frame was one byte, _pos=4 would be the byte, pos=5 is the checksum, where end stop reading
_response_FrameLength = (_pos - 4);
// reset state vars
_pos = 0;
_checksumTotal = 0;
return;
} else {
// add to packet array, starting with the fourth byte of the apiFrame
_response_FrameData[_pos - 4] = b;
_pos++;
}
}
}
}
