Skip to content

Instantly share code, notes, and snippets.

@ttchengab
Created November 11, 2016 09:44
Show Gist options
  • Save ttchengab/efd19cc45b66c88987a5dadaf731fcf6 to your computer and use it in GitHub Desktop.
Save ttchengab/efd19cc45b66c88987a5dadaf731fcf6 to your computer and use it in GitHub Desktop.
#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);
}
*/
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment