void motor_set_speed(int16_t speed)
{
if(speed < 0)
{
motor_direction = DIR_COUNTERCLOCKWISE;
commutation_table = ccw;
}
else if(speed > 0)
{
motor_direction = DIR_CLOCKWISE;
commutation_table = cw;
}
/* Saturation */
if((speed * motor_direction) > 100)
{
motor_speed = (100 * motor_direction);
}
else
{
motor_speed = (speed * motor_direction);
}
/* Starting up motor if speed is zero */
if(!motor_read_speed())
{
motor_start();
}
}
/**
* Register query instruction handler function.
*
* Used to ask the system from the outside about interesting
* information, like the current order or the current speed.
* @param[in,out] order The order that specifies which register
* (not real register) should be returned.
*/
void query_instruction(order_t *order) {
// Extract the instrution code
int instruction = (order->data[0] & 0xf0);
order_t *current_order = 0;
uint8_t current_order_size = 0;
if (DEBUG_ENABLE) {
debug_write_string_p(PSTR("order_functions.c : query_instruction()\n"));
}
switch (instruction) {
case 0x10: // left wheel Speed
io_obj_start();
io_put(motor_read_speed(WHEEL_LEFT));
io_obj_end();
break;
case 0x20: // right wheel Speed
io_obj_start();
io_put(motor_read_speed(WHEEL_RIGHT));
io_obj_end();
break;
case 0x30: // number of Orders in the Queue
io_obj_start();
io_put(queue_order_available() - 1); // -1 because Query will be counted
io_obj_end();
break;
case 0x40: // current Order
// Get the normal order (just get_current_order() would return this priority order)
if ((current_order = queue_get_current_normal_order())) {
// Find out how long this order is
current_order_size = order_size(current_order);
}
// Put the Size of the order as own object
io_obj_start();
io_put(current_order_size);
io_obj_end();
if (DEBUG_ENABLE) {
debug_write_integer(PSTR("order_functions.c : query_instruction() : current_order_size = "), current_order_size);
}
// send the Order data
if (current_order) {
uint8_t i = 0;
io_obj_start();
for (;i < current_order_size;i++) {
io_put(current_order->data[i]);
if (DEBUG_ENABLE) {
debug_write_integer(PSTR("order_functions.c : query_instruction() : put = "), current_order->data[i]);
}
}
io_obj_end();
}
break;
case 0x50: // left wheel time trigger value
io_obj_start();
io_put(timer_t_trigger_counter[WHEEL_LEFT] >> 8);
io_put(timer_t_trigger_counter[WHEEL_LEFT] & 0x00ff);
io_obj_end();
break;
case 0x60: // left wheel position trigger value
io_obj_start();
io_put(irq_p_trigger_position[WHEEL_LEFT] >> 8);
io_put(irq_p_trigger_position[WHEEL_LEFT] & 0x00ff);
io_obj_end();
break;
case 0x70: // right wheel time trigger value
io_obj_start();
io_put(timer_t_trigger_counter[WHEEL_RIGHT] >> 8);
io_put(timer_t_trigger_counter[WHEEL_RIGHT] & 0x00ff);
io_obj_end();
break;
case 0x80: // right wheel position trigger value
io_obj_start();
io_put(irq_p_trigger_position[WHEEL_RIGHT] >> 8);
io_put(irq_p_trigger_position[WHEEL_RIGHT] & 0x00ff);
io_obj_end();
break;
case 0x90: // running time
io_obj_start();
io_put(timer_1s_counter >> 8);
io_put(timer_1s_counter & 0x00ff);
io_obj_end();
break;
}
// Remove the priority order from the Queue
queue_clear_priority();
}
