//test1 - modified rover.c in demos.

// major changes made :)

// Changed (written mostly) by Stephen Stair (2/20/2003

#include <config.h>

#include <conio.h>

#include <unistd.h>

#include <dsensor.h>

#include <dmotor.h>

#include <time.h>

#include <sys/lcd.h>

#include <tm.h>

///////////////////////////////////////////////////////////////////////////////

//

// Functions

//

///////////////////////////////////////////////////////////////////////////////

wakeup_t sensor_press_wakeup(wakeup_t data);

#define my_fwd rev

#define my_rev fwd

#define my_fast MAX_SPEED

#define my_med (2*MAX_SPEED/3)

#define my_slow (MAX_SPEED/3)

int state;

int spd;

int bmps;

long lastsafe;

long safetime;

// states:

// 0='safe', slow (too many bumps

// 1=normal, med speed

// 2=crusin', not many bumps and safe for a long time!

int main(int argc, char *argv[]) {

state=1;

bmps=20;

lastsafe=get_system_up_time();

safetime=0;

cputs("wait");

lcd_refresh();

msleep(2000);

cputs("");

while (!shutdown_requested()) {

// update motors

if(state==0) spd=my_slow;

if(state==1) spd=my_med;

if(state==2) spd=my_fast;

motor_a_speed(spd); // go!

motor_c_speed(spd);

motor_a_dir(my_fwd);

motor_c_dir(my_fwd);

//cputs("fwwd ");

if (wait_event(&sensor_press_wakeup, 0) != 0) {

if(TOUCH_2 || (TOUCH_1 && TOUCH_3)) { // this gets priroity

bmps +=20; // big bump!

motor_a_dir(my_rev);

motor_c_dir(my_rev);

motor_a_speed(my_fast);

motor_c_speed(my_fast);

msleep(400);

motor_a_dir(my_fwd);

motor_c_dir(my_rev);

msleep(300);

motor_a_dir(my_fwd);

motor_c_dir(my_fwd);

lastsafe=get_system_up_time();

} else {

if(TOUCH_1) { // curve a bit out of the way

bmps+=5;

motor_c_dir(my_rev);

motor_c_speed(my_fast);

msleep(100);

lastsafe=get_system_up_time();

}

if(TOUCH_3) { // curve a bit out of the way

bmps+=5;

motor_a_speed(my_fast);

motor_a_dir(my_rev);

msleep(100);

lastsafe=get_system_up_time();

}

}

// after all that, process incoming stuff and update state.

safetime=get_system_up_time()-lastsafe;

if(bmps>0 && safetime>200) {bmps--;lastsafe=get_system_up_time();}

switch(state) {

case 0: // cautious

if(bmps<20) state=1; // somewhat happier

break;

case 1: // normal

if(bmps==0) state=2; // let's get overconfident!

if(bmps>40) state=0; // we're really unhappy

break;

case 2: // happy

if(bmps>0) state=1; // not happy anymore

break;

}

cputc_0(state+'0');

}

}

return 0;

}

wakeup_t sensor_press_wakeup(wakeup_t data) {

safetime=get_system_up_time()-lastsafe;

lcd_refresh();

return (TOUCH_1) || TOUCH_2 || (TOUCH_3) || (safetime>200);

}