//follows wall with set distance d
void wallfollow(float d,float t){
if(t<d){
if(t<(d/2)){
setm(0.0f,0.9f);
ledBlue();
}
else if(t<(d*2/3)){
setm(0.2f,0.8f);
ledBlue();
}
else{
setm(0.4f,0.7f);
ledBlue();
}
}
else{
if(t==d){
setm(.5f,.5f);
ledGreen();
}
else if(t>(d*2)){
setm(0.9f,0.0f);
ledRed();
}
else if(t>(d*3/2)){
setm(0.8f,0.2f);
ledRed();
}
else{
setm(0.7f,0.4f);
ledRed();
}
}
}
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();}
}
void turnback(float w){
setm(-.5f,.5f);
Wait(4.0f);
setm(-0.3f,-.3f);
Wait(w);
setm(0.0f,0.0f);
if(team==1){
team=0;
ledBlue();
}
else{
team=1;
ledRed();
}
Wait(2.0f);
}
void squaredance(void){
int p=0;
while(p<4){
setm(.5f,.5f);
if(ADCRead(adc[2])>.4f){
Wait(3.0f);
ledRed();
setm(0.0f,0.5f);
Wait(3.45f);
setm(.5f,.5f);
Wait(2.0f);
p+=1;
}
else{
ledBlue();
}
}
setm(0.0f,0.0f);
}
void feight(void){
while(1){
setm(.5f,.5f);
if(ADCRead(adc[2])>.4f){
ledRed();
Wait(3.0f);
setm(0.0f,0.7f);
Wait(3.0f);
setm(0.5f,0.5f);
Wait(2.0f);
setm(0.0f,0.7f);
Wait(3.0f);
setm(0.5f,0.5f);
Wait(2.0f);
}
else{
ledBlue();
}
}
}
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();
}
}
}
// Modify the LED colour depending on the current state and button state
void led() {
// If button has been pressed and it is possible to advance to the next state
if (digitalRead(BUTTON) == HIGH &&
STATE != STATE_NO_SD &&
STATE != STATE_SD_SETUP &&
STATE != STATE_DATA_CONCLUDE &&
STATE != STATE_ERROR) {
// Solid white
ledWhite();
}
// If button has not been pressed
else {
switch (STATE) {
case STATE_NO_SD:
// Pulsing red/orange
analogWrite(LED_RED, LED_ON);
analogWrite(LED_GREEN, pulse()/2);
analogWrite(LED_BLUE, LED_OFF);
break;
case STATE_SD_SETUP:
// Pulsing blue/magenta
analogWrite(LED_RED, pulse()/2);
analogWrite(LED_GREEN, LED_OFF);
analogWrite(LED_BLUE, LED_ON);
break;
case STATE_STANDBY:
// Pulsing blue/cyan
analogWrite(LED_RED, LED_OFF);
analogWrite(LED_GREEN, pulse());
analogWrite(LED_BLUE, LED_ON);
break;
case STATE_DATA_SETUP:
// Colour controlled within setup function
break;
case STATE_DATA_COLLECT:
// Solid green
ledGreen();
break;
case STATE_DATA_CONCLUDE:
// Pulsing blue/magenta
analogWrite(LED_RED, pulse()/2);
analogWrite(LED_GREEN, LED_OFF);
analogWrite(LED_BLUE, LED_ON);
break;
case STATE_WARNING:
// Blinking red/yellow
if ((PULSE_COUNT >= LED_ON && PULSE_COUNT <= 63) ||
(PULSE_COUNT >= 128 && PULSE_COUNT <= 191)) ledRed();
else ledYellow();
break;
case STATE_ERROR:
// Blinking red/white
if ((PULSE_COUNT >= LED_ON && PULSE_COUNT <= 63) ||
(PULSE_COUNT >= 128 && PULSE_COUNT <= 191)) ledRed();
else ledWhite();
break;
} // End of switch
} // End of if
// Increment PULSE_COUNT
PULSE_COUNT = (PULSE_COUNT + 1)%256;
} // End of led
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");
}
