// Blink the LED counting down to data collection
void ledCountdown() {
// If a countdown is specified (i.e. not zero)
if (SETTINGS.countdown) {
uint32_t countdownStart;
// Make the LED yellow for 250 milliseconds
countdownStart = millis();
while (sdInserted() &&
millis() - countdownStart < 250 &&
NEXT_STATE != STATE_DATA_CONCLUDE) {
ledYellow();
} // End of while
// Blink the LED green for 150 milliseconds once every second
countdownStart = millis();
ledGreen();
while (sdInserted() &&
millis() - countdownStart < 1000 * SETTINGS.countdown &&
NEXT_STATE != STATE_DATA_CONCLUDE) {
if ((millis() - countdownStart)%1000 > 149) ledYellow();
else ledGreen();
} // End of while
} // End of if
} // End of ledCountdown
//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 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();
}
}
}
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;
}
}
// 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");
}
