void cmd_rpmset(char* params){
int16_t left = params[0] << 8; left += params[1];
int16_t right = params[2] << 8; right += params[3];
left -= 10000;
right -= 10000;
motors_set(MOTORS_LEFT, left);
motors_set(MOTORS_RIGHT, right);
}
int main( void ){
DDRC = 0xDB;
//PORTC = 0x7E;;
PORTC = 0xE7;
uint8_t error;
Packet packet;
Queue* packets;
packets = Packets_getQueue();
// initialize globals
globals_init();
// initialize Time
time_init();
TimeResult tr;
uint32_t previous = 0;
uint32_t watchdog = 0;
// initialize usart
usart_init();
// initialize motors
motors_init();
motors_set(MOTORS_LEFT, 0);
motors_set(MOTORS_RIGHT, 0);
// initialize analog input
analoginput_init();
// enable interrupts
sei();
// loop forever
while(1){
error = Packets_getError();
if (error){
Packets_sendError(ERROR_PACKETS, error);
continue;
}
if (packets->count > 0){
error = 0x00;
packet = Packets_getNext();
error = Queue_getError(packets);
if (error){
Packets_sendError(ERROR_QUEUE, error);
continue;
}
handle_packet(packet);
watchdog = time_get_time();
}
tr = time_get_time_delta(previous);
// every 20ms, calculate rpm
if (get_time_in_ms(tr.delta) > 20){
previous = tr.previous;
motors_tick();
send_status();
}
tr = time_get_time_delta(watchdog);
if (get_time_in_ms(tr.delta) > 500){
motors_set(MOTORS_LEFT, 0);
motors_set(MOTORS_RIGHT, 0);
}
}
}
// This function will be run once, when the command is started
jobstatus testjob1_start(Command *cmd){
// Start motors moving
motors_set(80, 80);
return JOB_NORMAL;
}
// This function will be run once, when the command is over.
// This is where all cleanup should be done (ie turning off outputs)
jobstatus testjob1_stop(Command *cmd){
motors_set(0, 0);
return JOB_CLEANUP_COMMAND;
}
