Пример #1
0
static void
rest_servos(void)
{
    servo_update(&lead_state, 100);
    servo_update(&backup_state, 0);
    servo_update(&bass_state, 49);
    triangle_rest(&triangle_state);
}
Пример #2
0
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);
}
Пример #3
0
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]);
}
Пример #4
0
/**
   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;
}
Пример #5
0
void servo_loop()
{
	unsigned int idx;
	for (idx = 0; idx < ARR_SIZE(servoPins); idx++)
		servo_update(idx);
}
Пример #6
0
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;
}