void followLine(void){
// put the values of the line sensor into the 'line' array
LineSensorReadArray(gls, line);
if((line[3]>0.5)&&(line[4]>0.5)){
SetMotor(leftMotor, 1);
SetMotor(rightMotor, 1);
}
else if (line[5]>0.5&&line[4]>0.5){
SetMotor(leftMotor, .8);
SetMotor(rightMotor, .4);
}
else if (line[2]>0.5&&line[3]>0.5){
SetMotor(leftMotor, .4);
SetMotor(rightMotor, .8);
}
else if (line[7]>0.5&&line[6]>0.5){
SetMotor(leftMotor, .3);
SetMotor(rightMotor, -.25);
}
else if (line[0]>0.5&&line[1]>0.5){
SetMotor(leftMotor, -.25);
SetMotor(rightMotor, .3);
}
else if (line[6]>0.5&&line[5]>0.5){
SetMotor(leftMotor, .6);
SetMotor(rightMotor, .1);
}
else if (line[1]>0.5&&line[2]>0.5){
SetMotor(leftMotor, .6);
SetMotor(rightMotor, .1);
}
}
bool mostDark(void){
int i = 0;
int count = 0;
LineSensorReadArray(gls, line);
for (i = 0; i<8; i+=1){
if(line[i]>0.5){count += 1;}
}
if(count>4&&count<9) {return true;}
else {return false;}
}
bool linePresent(void){
int i = 0;
int count = 0;
LineSensorReadArray(gls, line);
for (i = 0; i<8; i+=1){
if(line[i]>0.5){count += 1;}
}
if(count>1&&count<4) {return true;}
else {return false;}
}
int turnline(void){
float line[8];
LineSensorReadArray(gls, line);
int i=0;
int total = 0;
for (i = 0; i < 8; i++) {
if(line[i] > 1000)
total+=1;
}
return total;
}
void edgefollow(){
float line[8];
LineSensorReadArray(gls, line);
if(line[0]>100) {
setm(.2f,.2f);ledGreen();}
else if(line[7]>100){
setm(.6f,-.1f);ledBlue();}
else{
setm(-.1f,.6f);ledRed();}
}
int main(void) {
initIRSensor();
initMotor();
initGPIOLineSensor();
initServo();
SetServo(servo,0);
stage = 0;
//while(1){followLine();}
//while(1){followWall();}
//SetMotor(leftMotor, 1); SetMotor(rightMotor, 1);
while(true){
LineSensorReadArray(gls, line);
if (stage==0){ //start state
if(line[0]<0.5&&line[1]<0.5&&line[2]<0.5&&line[3]<0.5&&line[4]<0.5&&line[5]<0.5&&line[6]<0.5&&line[7]<0.5) {
followWall();
}else{
followLine();
}
}else if(stage==1){
//once 90degree turn passed
// SetMotor(leftMotor, 1);
// SetMotor(rightMotor, -1);
SetPin(PIN_F2, 1);
followWall();
if (wallPresent()) {followWall();}
else {
findLine();
}
SetPin(PIN_F2, 0);
}else if (stage==2){
//once line found again after walled section
SetPin(PIN_F1, 1);
if((line[0]<0.5&&line[1]<0.5&&line[2]<0.5&&line[3]<0.5&&line[4]<0.5&&line[5]<0.5&&line[6]<0.5&&line[7]<0.5)||
(mostDark())) { //line[0]>0.5&&line[1]>0.5&&line[2]>0.5&&line[3]>0.5&&line[4]>0.5&&line[5]>0.5&&line[6]>0.5&&line[7]>0.5)
findEnd();
}else{
followLine();
};
SetPin(PIN_F1, 0);
}else{//end of course look for flag
findObject();
break;
}
}
}
int online(){
float line[8];
LineSensorReadArray(gls, line);
int total = 0;
int i=0;
for (i = 0; i < 8; i++) {
Printf("%.2f ", line[i]);
if(line[i] > 1000)
total+=1;
}
if(total > 5){
ledGreen();
return 1;}
else{
// ledGreen();
return 0;
}
}
void lineSensorDemo(void) {
Printf("Press:\nany key-read line sensor\n");
Printf("any key after read begins-quit\n");
while(!KeyWasPressed()) {
int i;
float line[8];
LineSensorReadArray(ls, line);
Printf("Line Sensor: [");
for (i=0; i < 8; i++) {
Printf("%01.4f ", line[i]);
}
Printf("\b]\r");
}
Printf("\n");
}
void gpioLineSensorDemo(void) {
Printf("Press any key to quit\n");
// loop as long as the user doesn't press a key
while (!KeyWasPressed()) {
int i;
float line[8];
// put the values of the line sensor into the 'line' array
LineSensorReadArray(gls, line);
Printf("Line Sensor: [");
for (i = 0; i < 8; i++) {
Printf("%.2f ", line[i]);
}
Printf("\b] \r");
}
Printf("\n");
}
void gpioLineSensorDemo(void) {
Printf("Press:\nany key-read line sensor\n");
Printf("any key after read begins-quit\n");
while(!KeyWasPressed()) {
int i;
float line[8];
char lineSensorBits = LineSensorRead(gls, 0.8f);
LineSensorReadArray(gls, line);
//Printf("Line Sensor: [%d]\r", lineSensorBits);
Printf("\r");
for (i=0; i < 8; i++) {
Printf("%d ", lineSensorBits & (1 << i) ? 1 : 0);
//Printf("%1.4f ", line[i]);
}
Printf(" ");
}
//Printf("\n");
}
void lineFollo(void){
int i;
float line[8];
float total = 0.0f;
float tot=0.0f;
LineSensorReadArray(gls,line);
for (i = 0; i < 4; i++) {
total+=((i*1.0f))*line[i];
}
for (i = 4; i < 8; i++) {
tot+=((i*1.0f))*line[i];
}
if(total > 1000)
{
if(tot<1000){
setm(.5f,.1f);
ledRed();
}
else{
setm(.2f,.2f);
ledGreen();
}
}
else{
if(tot > 1000){
setm(.1f,.5f);
ledBlue();
}
else{
setm(.2f,.2f);
ledGreen();
}
}
}
void lineFollow(void) {
int i;
float line[8];
float total = 0.0f;
LineSensorReadArray(gls, line);
Printf("Line Sensor: [");
for (i = 0; i < 8; i++) {
Printf("%.2f ", line[i]);
if(line[i] < 1000){
total+=((i*1.0f)-3.5f)*line[i];
}
else{
total+=((i*1.0)-3.5f)*5;
}
}
Printf("\b] \r");
if(total == 0){
turn=0;
ledGreen();
setm(.25f,.3f);
}
else if(total < 0){
if(total > (-20.0f)){
if(turn==0){
ledGreen();
setm(.25f,.30f);
}
else{
turn=0;
}
}
else if(turn ==1){
ledBlue();
if (total < (-30))
{ ledBlue();setm(.45f, .00f);}
else if(total > (-25))
{ setm(.35f,.00f);ledBlue();}
else
{ setm(.25f, .00f);ledBlue();}
}
else{
turn=1;
}
}
else{
if(total < (20.0f)){
if(turn==0){
ledGreen();
setm(.25f,.30f);
}
else{
turn=0;
}
}
else{
ledRed();
if(turn==2){
if(total > 30)
{ ledRed();setm(.00f,.45f);}
else if(total < 25)
{ setm(.00f,.35f);ledRed();}
else
{ setm(.00f,.25f);ledRed();}
}
else{
turn=2;
}
}
}
Printf("\n");
}
