static void rest_servos(void) { servo_update(&lead_state, 100); servo_update(&backup_state, 0); servo_update(&bass_state, 49); triangle_rest(&triangle_state); }
void loop() { unsigned long now = millis(); unsigned long delta = now - last_update; last_update = now; servo_update(pan_servo,delta); servo_update(tilt_servo,delta); delay(UPDATE_INTERVAL); }
void set_servos_from_network(Msg_t *update) { if (update->pingmsg.id != TYPE_UPDATE) { return; } for (int i=1; i<6; i++) servo_update(i, update->respmsg.values[i]); }
/** Initialize. al is additional latency */ SERVO_T *servo_new(dcell *merr, const dmat *ap, int al, double dt, const dmat *ep){ SERVO_T *st=calloc(1, sizeof(SERVO_T)); if(ap){ st->ap=ddup(ap); }else{ st->ap=dnew(2,1); st->ap->p[0]=1; } if(st->ap->nx<2){ dresize(st->ap, 2, 1);//2 element to ensure we keep integrator history. } st->mint=cellnew(st->ap->nx, 1); st->dt=dt; st->al=al; st->merrhist=cellnew(st->al+1, 1); servo_update(st, ep); if(merr && merr->nx!=0 && merr->ny!=0 && merr->p[0]){ servo_init(st, merr); } return st; }
void servo_loop() { unsigned int idx; for (idx = 0; idx < ARR_SIZE(servoPins); idx++) servo_update(idx); }
int main() { int which_to_update = 6; // start greater than 5 so we get data uint32_t data[5]; // Array to hold ADC data // Initialize all the things LED_init(); systick_init(400000); // Timer goes off 10 times per second USART2_init(); USART3_init(); button_init(); ADC_init(); servo_init(); DMA_init(); /* Enable interrupts */ __asm (" cpsie i \n" ); /* Main program loop */ while(1) { // State specific behavior (every time) switch (mode_state) { case CONFIGURE_S: // Don't do anything break; case COMMAND_S: { /* Send in two cases: * a) we're not waiting for a packet * b) the update flag is set (every second, because sometimes packets get dropped * and we don't want to wait forever) * * When we send a byte, we'll send whichever is next in the sequence (the current one * is stored in the local variable (in main) which_to_update). When it goes over 5, * we read in all the data for the next round of packets. */ if (!waiting_to_recv_packet || send_update_f) { // if (send_update_f) { if (which_to_update > 5) { // finished updating ADC_read(data); which_to_update = 1; } // We will be waiting for a packet back, so set this ahead of time waiting_to_recv_packet=1; // A new packet will be inbound, so reset the offset to 0 (in case it got messed up before) recv_offset = 0; update_server(which_to_update, data); which_to_update++; send_update_f = 0; } break; } case CLIENT_S: { /* * Update the servos on the high tick of this flag. That is set in systick, and happens 10 times * per second */ if (update_servos_from_server_f) { // We're about to receive a response packet, so we reset the recv_offset to 0 recv_offset = 0; waiting_to_recv_packet = 1; update_servos(); update_servos_from_server_f = 0; } break; } default: break; } // Every time, regardless of state: // If we received a packet, print it if (received_new_packet) { // switch (recv_msg.pingmsg.type) { // case TYPE_PING: // print_string("[PING,id="); // printUnsignedDecimal(recv_msg.pingmsg.id); // print_string("]\n"); // break; // case TYPE_UPDATE: // print_string("[UPDATE,id="); // printUnsignedDecimal(recv_msg.respmsg.id); // print_string(",average="); // printUnsignedDecimal(recv_msg.respmsg.average); // print_string(",{"); // for (int i=0; i<CLASS_SIZE_MAX; i++) { // print_string(" "); // printUnsignedDecimal(recv_msg.respmsg.values[i]); // } // print_string("}]\n\r"); // break; // default: // break; // } // Reset the flag received_new_packet = 0; // If we're in client mode, set the servo values to those from the server if (mode_state == CLIENT_S) { if (recv_msg.respmsg.type == TYPE_UPDATE) { for (int i=1; i<=5; i++) servo_update(i, recv_msg.respmsg.values[i]); } } } // After switching states, update leds if (update_leds_f) { switch (mode_state) { case CONFIGURE_S: LED_update(LED_BLUE_ON|LED_ORANGE_OFF); // Configuration: // $$$ (escape sequence) // set ip dhcp 1 (get IP address with dhcp) // set ip host 172.16.1.10 (set remote IP) // set ip remote 8004 // set wlan join 1 (try to connect to stored access point) // set wlan auth 4 (set to WPA2-PSK) // set wlan phrase ENGS62wifi // set wlan ssid ENGS62 // save // reboot break; case CLIENT_S: LED_update(LED_BLUE_OFF|LED_ORANGE_ON); break; case COMMAND_S: LED_update(LED_BLUE_ON|LED_ORANGE_ON); waiting_to_recv_packet = 0; break; } update_leds_f = 0; } // If in debug mode, print ADC data to the console if (DEBUG && test_flag) { test_flag = 0; uint32_t data[5]; // Initialize the data array to 0 for clarity for (int i=0; i<5; i++) { data[i] = 0; } ADC_read(data); for (int i=0; i<5; i++) { printUnsignedDecimal((uint16_t)data[i]); print_string("\n"); print_string("\r"); } print_string("-----------\n"); } } /* We'll never reach this line */ return 0; }