robotics.cpp

#include "main.h"

u32 ticks_img = 0;
u32 ticks_sec_img = 0;
u16 servo_pos = 750;
u8 speed = 20;
u16 speed_indic[3] = {RGB888TO565(0xC72929), RGB888TO565(0xFFC72C), RGB888TO565(0x40CA77)};

//Button listeners
/*
void button1_keydown(void) {led_toggle(LED1);}
void button2_keydown(void) {led_toggle(LED2);}
void button3_keydown(void) {led_toggle(LED3);}
*/
int queue[105][2]; 
int front;
int back;
# define N 4410 //The average value of a single value in ccd buffer after u take 105 samples of sum of 42 numbers
void q_initialize() {
    front = back = 0;
}
void q_push(int left, int right) {
    queue[back][0] = left; // left side moving average
    queue[back][1] = right; // right side moving average
		if(back < 104) {back++;}
		else {back = 0;}
}
int q_get_left() { // the number about to be deleted from the moving average queue
    return queue[front][0];
}
int q_get_right() {// the number about to be deleted from the moving average queue
    return queue[front][1];
}
void q_pop() {
   if(front < 104) {front++;}
		else {front = 0;} 
}
void change_speed(void) {
	static u8 count = 0;
	count++;
	speed = 20; //case 0
	switch (count % 3) {
		case 2: //speed = 60
			speed += 20;
		case 1: //speed = 40
			speed += 20;
	}
	tft_fill_area(72, 72, 12, 12, speed_indic[count % 3]);
}

//UART listener
void uart3_listener(const u8 byte) {
	u8 ledno = byte - (u8)'1';
	if (ledno >= 3) return;
	led_toggle((LED_ID)ledno);
}


int main() {
	ticks_init();
	led_init();
	gpio_init();
	pneumatic_init();
	
	button_init();
	set_keydown_listener(BUTTON2, &change_speed);
	
	tft_init(2, BLACK, WHITE);
	/*
	tft_init(3, BLACK, RGB888TO565(0xFF0080));
	tft_prints(0, 0, "Hello World!");
	tft_set_text_color(WHITE);
	tft_prints(0, 12, "My name is Pang.");
	tft_prints(0, 24, "I am a single & toxic guy :)");
	tft_prints(0, 60, "Seconds:");
	tft_prints(46, 60, "0");
	tft_prints(0, 72, "Servo:");
	tft_prints(46, 72, "750");
	//tft_set_font(&serifGothic_16ptFontInfo);
	//tft_prints(0, 92, "War Dragon");
	//tft_set_font(&microsoftSansSerif_8ptFontInfo);
	//tft_print_image(0, 120, Logo);
	
	tft_fill_area(72, 72, 12, 12, RED);
	
	tft_fill_area(10, 96, 57, 108, WHITE);
	tft_fill_area(12, 98, 55, 106, BLACK);
	
	uart_init(COM3, 115200);
	uart_tx(COM3, "Test\n");
	uart_interrupt_init(COM3, &uart3_listener);
	*/
	
	servo_init(143, 10000, 0);
	adc_init();
	uart_init(COM1, 115200);
	linear_ccd_init();
	
	q_initialize();
	
	int sumleft = 0, sumright= 0;
	int prevavg;
	int avgleft = 0, avgright= 0; 
	int i = 0;
	//int servoavg;
	
	while (1) {
		if (ticks_img != get_real_ticks()) {
			ticks_img = get_real_ticks();
			if (ticks_img % 10 == 0) continue;
				linear_ccd_read();
			//for(int i=0;i<128;i++) uart_tx(COM1,"%d ",linear_ccd_buffer1[i]);	
			sumleft = 0;
			sumright = 0;
			for(int j = 0; j < 42; j ++)
			{
				sumleft= sumleft + linear_ccd_buffer1[j];
				sumright = sumright + linear_ccd_buffer1[86+j];
			}
			if(i < 105)
			{ 
				avgleft = avgleft + sumleft/N;
				avgright = avgright + sumright/N;
				i ++;
			}
			else 
			{
					prevavg = avgright - avgleft;
					uart_tx(COM1, "%d\n", avgright-avgleft);
					avgleft = avgleft - q_get_left() + sumleft/N;
					avgright = avgright - q_get_right() + sumright/N;
					q_pop();
				//test
					//if(ticks_img%10 == 0)
					//{
						if(avgright-avgleft > 30)
						{
							uart_tx(COM1, "Turn Right\n");
							
						}
						else if(avgright-avgleft < -30 )
						{
							uart_tx(COM1, "Turn Left\n");
							
						}
						else
						{
							uart_tx(COM1, "Walk Straight\n");
						}
					//	uart_tx(COM1, "%d\n",avgright);
					//}
					
				/*	if((avgright - avgleft > 30 && prevavg> 30)||(avgright - avgleft < -30 && prevavg < -30))
					{
						if( (avgright - avgleft > 150)  servoavg = 150;
						else if (avgright - avgleft <-150) servoavg = -150;
						else servoavg = avgright - avgleft;
						servo_control( SERVO1, 750 + 2*servoavg);
						
					}
				*/
			}
			q_push(sumleft/N, sumright/N);
				
				
				
			
				//button_update();
				
				/*
				if (read_button(BUTTON1) == 0 && servo_pos < 1050) {
					servo_pos += speed;
					tft_fill_area(46, 72, 25, 12, BLACK);
					tft_prints(46, 72, "%d", servo_pos);
				}
				
				if (read_button(BUTTON3) == 0 && servo_pos > 450) {
					servo_pos -= speed;
					tft_fill_area(46, 72, 25, 12, BLACK);
					tft_prints(46, 72, "%d", servo_pos);
				}
				*/
			}
			/*
			if (ticks_img % 1000 == 0) {
				tft_fill_area(46, 60, 25, 12, BLACK);
				tft_prints(46, 60, "%d", get_second_ticks());
			}
			*/
			/*if (ticks_img % 10 == 0) {
				for (u8 i=0; i<128; i++)
					tft_put_pixel(i, 159 - linear_ccd_buffer1[i], BLACK);
				linear_ccd_read();
				for (u8 i=0; i<128; i++)
					tft_put_pixel(i, 159 - linear_ccd_buffer1[i], WHITE);
			}
		}*/
		/*
		servo_control(SERVO1, servo_pos);
		if (read_gpio(GPIOA, GPIO_Pin_11)) {
			led_on(LED1);
			pneumatic_control(GPIOC, GPIO_Pin_0, 1);
		} else {
			led_off(LED1);
			pneumatic_control(GPIOC, GPIO_Pin_0, 0);
		}
		*/
	}
}