#include "config.h"
#include "pt_cornell_1_2.h"
#include "tft_master.h"
#include "tft_gfx.h"
#include <stdlib.h>
// qcif format; 176 x 144
#define midpoint 88
#define follow_dist 20
#define reverse_dist 30
static struct pt pt_main, pt_sim;
unsigned char inc_flag;
unsigned char state_lr;
unsigned char state_fb;
// location of markers, 0 and 2 for distance, 1 for direction
unsigned char loc_x[3];
// movement functions
void stop() {
mPORTBSetBits(BIT_5);
mPORTBSetBits(BIT_7);
if (state_fb == 1) {
tft_fillTriangle(160, 10, 130, 70, 190, 70, ILI9340_BLACK); // up
tft_drawTriangle(160, 10, 130, 70, 190, 70, ILI9340_WHITE); // up
} else if (state_fb == 2) {
tft_fillTriangle(160, 230, 130, 170, 190, 170, ILI9340_BLACK); // down
tft_drawTriangle(160, 230, 130, 170, 190, 170, ILI9340_WHITE); // down
}
state_fb = 0;
}
void moveForward() {
mPORTBClearBits(BIT_5);
mPORTBSetBits(BIT_7);
if (state_fb == 0) {
tft_fillTriangle(160, 10, 130, 70, 190, 70, ILI9340_WHITE); // up
} else if (state_fb == 2) {
tft_fillTriangle(160, 10, 130, 70, 190, 70, ILI9340_WHITE); // up
tft_fillTriangle(160, 230, 130, 170, 190, 170, ILI9340_BLACK); // down
tft_drawTriangle(160, 230, 130, 170, 190, 170, ILI9340_WHITE); // down
}
state_fb = 1;
}
void moveBackward() {
mPORTBClearBits(BIT_7);
mPORTBSetBits(BIT_5);
if (state_fb == 0) {
tft_fillTriangle(160, 230, 130, 170, 190, 170, ILI9340_WHITE); // down
} else if (state_fb == 1) {
tft_fillTriangle(160, 10, 130, 70, 190, 70, ILI9340_BLACK); // up
tft_drawTriangle(160, 10, 130, 70, 190, 70, ILI9340_WHITE); // up
tft_fillTriangle(160, 230, 130, 170, 190, 170, ILI9340_WHITE); // down
}
state_fb = 2;
}
void straight() {
mPORTASetBits(BIT_2);
mPORTASetBits(BIT_3);
if (state_lr == 1) {
tft_fillTriangle(35, 120, 95, 150, 95, 90, ILI9340_BLACK); // left
tft_drawTriangle(35, 120, 95, 150, 95, 90, ILI9340_WHITE); // left
} else if (state_lr == 2) {
tft_fillTriangle(285, 120, 225, 150, 225, 90, ILI9340_BLACK); // right
tft_drawTriangle(285, 120, 225, 150, 225, 90, ILI9340_WHITE); // right
}
state_lr = 0;
}
void moveRight() {
mPORTAClearBits(BIT_2);
mPORTASetBits(BIT_3);
if (state_lr == 0) {
tft_fillTriangle(35, 120, 95, 150, 95, 90, ILI9340_WHITE); // left
} else if (state_lr == 2) {
tft_fillTriangle(35, 120, 95, 150, 95, 90, ILI9340_WHITE); // left
tft_fillTriangle(285, 120, 225, 150, 225, 90, ILI9340_BLACK); // right
tft_drawTriangle(285, 120, 225, 150, 225, 90, ILI9340_WHITE); // right
}
state_lr = 1;
}
void moveLeft() {
mPORTAClearBits(BIT_3);
mPORTASetBits(BIT_2);
if (state_lr == 0) {
tft_fillTriangle(285, 120, 225, 150, 225, 90, ILI9340_WHITE); // right
} else if (state_lr == 1) {
tft_fillTriangle(35, 120, 95, 150, 95, 90, ILI9340_BLACK); // left
tft_drawTriangle(35, 120, 95, 150, 95, 90, ILI9340_WHITE); // left
tft_fillTriangle(285, 120, 225, 150, 225, 90, ILI9340_WHITE); // right
}
state_lr = 2;
}
/*
// computer vision algorithm
unsigned char[] find_markers(void* image_ptr) {
unsigned char markers[3];
// TODO
return markers;
} */
static PT_THREAD (protothread_move(struct pt *pt)) {
PT_BEGIN(pt);
while(1) {
PT_YIELD_UNTIL(pt, inc_flag);
if (loc_x[2] - loc_x[0] > reverse_dist) {
moveBackward();
if (-2 > loc_x[1] - midpoint)
moveRight();
else if (loc_x[1] - midpoint > 1)
moveLeft();
else
straight();
tft_fillCircle(160, 120, 30, ILI9340_RED);
} else if (loc_x[2] - loc_x[0] < follow_dist) {
moveForward();
if (-2 > loc_x[1] - midpoint)
moveLeft();
else if (loc_x[1] - midpoint > 1)
moveRight();
else
straight();
tft_fillCircle(160, 120, 30, ILI9340_GREEN);
} else {
stop();
straight();
tft_fillCircle(160, 120, 30, ILI9340_BLACK);
}
inc_flag = 0;
}
PT_END(pt);
}
// simulate vision algorithm output
unsigned int counter;
static PT_THREAD (protothread_simulate(struct pt *pt)) {
PT_BEGIN(pt);
while(1) {
counter++;
if (counter < 2*15) {
// stop
loc_x[0] = 75;
loc_x[1] = 88;
loc_x[2] = 100;
}
else if (counter < 4*15) {
// straight forward
loc_x[0] = 80;
loc_x[1] = 88;
loc_x[2] = 95;
} else if (counter < 6*15) {
// left forward
loc_x[0] = 80;
loc_x[1] = 84;
loc_x[2] = 95;
} else if (counter < 8*15) {
//right forward
loc_x[0] = 80;
loc_x[1] = 92;
loc_x[2] = 95;
} else if (counter < 14*15) {
// stop
loc_x[0] = 75;
loc_x[1] = 88;
loc_x[2] = 100;
}
else if (counter < 16*15) {
// straight backward
loc_x[0] = 70;
loc_x[1] = 88;
loc_x[2] = 105;
} else if (counter < 18*15) {
// left backward
loc_x[0] = 70;
loc_x[1] = 84;
loc_x[2] = 105;
} else if (counter < 20*15) {
// right backward
loc_x[0] = 70;
loc_x[1] = 92;
loc_x[2] = 105;
} else {
// stop
loc_x[0] = 75;
loc_x[1] = 88;
loc_x[2] = 100;
}
inc_flag = 1;
PT_YIELD_TIME_msec(33);
}
PT_END(pt);
}
// === Main ======================================================
void main(void) {
ANSELA = 0; ANSELB = 0;
INTEnableSystemMultiVectoredInt();
PT_setup();
inc_flag = 0;
counter = 0;
state_fb = 0;
state_lr = 0;
loc_x[0] = 75;
loc_x[1] = 88;
loc_x[2] = 100;
// Right, pin 9 (A2)
mPORTAClearBits(BIT_2);
mPORTASetPinsDigitalOut(BIT_2);
mPORTASetBits(BIT_2);
// Left, pin 10 (A3)
mPORTAClearBits(BIT_3);
mPORTASetPinsDigitalOut(BIT_3);
mPORTASetBits(BIT_3);
// Forward, pin 14 (B5)
mPORTBClearBits(BIT_5);
mPORTBSetPinsDigitalOut(BIT_5);
mPORTBSetBits(BIT_5);
// Backward, pin 15 (B6)
mPORTBClearBits(BIT_7);
mPORTBSetPinsDigitalOut(BIT_7);
mPORTBSetBits(BIT_7);
// init tft
tft_init_hw();
tft_begin();
tft_setRotation(1); // 320x240
tft_fillScreen(ILI9340_BLACK);
// movement circle
tft_drawCircle(160, 120, 31, ILI9340_WHITE);
// direction triangles
tft_drawTriangle(35, 120, 95, 150, 95, 90, ILI9340_WHITE); // left
tft_drawTriangle(285, 120, 225, 150, 225, 90, ILI9340_WHITE); // right
tft_drawTriangle(160, 10, 130, 70, 190, 70, ILI9340_WHITE); // up
tft_drawTriangle(160, 230, 130, 170, 190, 170, ILI9340_WHITE); // down
// round-robin scheduler for threads
while (1){
PT_SCHEDULE(protothread_move(&pt_main));
PT_SCHEDULE(protothread_simulate(&pt_sim));
}
}