void Entity::move_down_flat(float force) {
move_dist(fvec3(0.0,
-get_speed(force),
0.0));
}
void Entity::move_down(float force) {
fvec3 up = angle.get_up();
move_dist(-get_speed(force)*up);
}
void Entity::move_right(float force) {
fvec3 forward = angle.get_forward();
move_dist(fvec3(-forward.z * get_speed(force),
0,
forward.x * get_speed(force)));
}
void Entity::move_backward_flat(float force) {
move_dist(angle.get_forward() * (-get_speed(force)));
}
void Entity::move_forward_flat(float force) {
// override this if flying/on land
move_dist(angle.get_forward() * get_speed(force));
}
//Handler for OI, moved from control.
void oi_system()
{
movement_data_t movement_data;
rotation_data_t rotation_data;
enum {
oi_command_init = 0,
oi_command_move = 1,
oi_command_rotate = 2,
oi_command_play_song = 3,
oi_command_dump = 4,
oi_command_end_sequence = 5
} oi_command = usart_rx();
txq_enqueue(oi_command);
switch (oi_command) {
case oi_command_init:
oi_init(&(control.oi_state));
break;
case oi_command_move:
if(rx_frame()) {
r_error(error_frame,"Move should not have multiple frames.");
}
struct {
uint16_t speed;
uint16_t dist;
bool stream;
} *move_data = (void *) &control.data;
struct {
uint16_t dist;
uint8_t flag;
} *response_move = (void *) &control.data;
#warning "Stream functionality to be implemented later."
//Stream returns the distance traveled
//lcd_puts ("In OI subsystem"); //debug
movement_data = move_dist(&(control.oi_state), move_data->dist, move_data->speed);
response_move->dist = movement_data.travelled;
response_move->flag = movement_data.flag;
if (movement_data.flag != 0) {
movement_data = move_dist(&(control.oi_state), -20, move_data->speed);
response_move->dist += movement_data.travelled;
}
control.data_len = 3;
tx_frame(false);
break;
case oi_command_rotate:
if (rx_frame()) {
r_error(error_bad_message, "Rotate should only have one data frame.");
}
int16_t *angle = &(control.data[0]); // TODO: test if this is the right number of bytes
if (control.data_len != 2/*sizeof(*angle)*/) { // TODO: hardcoding to debug
r_error(error_bad_message, "Received too much data with rotate "
"message.");
}
struct {
uint16_t rotation;
} *response_rotation = (void *) &control.data;
rotation_data = turn(&(control.oi_state), *angle);
response_rotation->rotation = rotation_data.rotated;
control.data_len = 2;
tx_frame(false);
break;
//Sing me a song.
case oi_command_play_song:
#warning "oi_command_play_song is deprecated"
;
//assuming that we get two data frames, the first containing the notes and the second containing the durations.
int j;
struct {
uint8_t n; //The number of notes present in the song
//char data[n];
uint8_t index;
} *song_data = (void *) &control.data;
// int j;
while(rx_frame()) { //this should happen twice please
char tmp_notes[song_data->n];
char tmp_durs[song_data->n];
for(j =0; j<song_data->n; j++) {
//tmp_notes[n]; TODO: broken
#warning "oi_command_play_song not implemented"
}
}
break;
case oi_command_dump:
//copies all of the data from OI_UPDATE and transmits to Control.
oi_update(&(control.oi_state));
memcpy(control.data, &control.oi_state, sizeof(control.oi_state));
control.data_len = sizeof(control.oi_state);
tx_frame(false);
break;
case oi_command_end_sequence:
#warning "oi_command_end_sequence not implemented"
// Switch power LED off
oi_set_leds(1, 1, 0, 0);
wait_ms(50);
// Switch power LED to orange
oi_set_leds(1, 1, 170, 255);
wait_ms(50);
// Switch power LED off
oi_set_leds(1, 1, 0, 0);
wait_ms(50);
// Switch power LED to yellow
oi_set_leds(1, 1, 70, 255);
wait_ms(50);
// Switch power LED off
oi_set_leds(1, 1, 0, 0);
wait_ms(50);
// Switch power LED back to default state (from oi_init())
oi_set_leds(1, 1, 7, 255);
// Play song
songs_load(RICK_ROLL);
break;
default:
r_error(error_bad_message, "Bad OI Command");
break;
}
}
