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; } }