**/\* simple forward-back test Grant 2/5/05 \*/**

**#define DELTA 0 /\* direction adjustment: + = more turn to left, - =
more to right \*/**

**simple_straight(1);**

**sleep(1.);**

**ao();sleep(.2);**

**simple_straight(-1);**

**sleep(1.);**

**ao();**

**printf(\"Done\\n\");**

**}**

**void simple_straight(int direction)**

**{**

**int l_speed=SPEED\*direction, r_speed=(SPEED + DELTA)\*direction;**

**motor( L_MOTOR, l_speed);**

**motor(R_MOTOR, r_speed);**

**}**

**/\* simple forward-back test1 - Gradual start Grant 2/5/05 \*/**

**#define DELTA 0 /\* direction adjustment: + = more turn to left, - =
more to right \*/**

**#define SPEED 75 /\* nominal speed \*/**

**/\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\* Robot port configuration
\*\*\*\*\*\*\*\*\*\*\*/**

**#define R_MOTOR 2 /\* motor port 2 \*/**

**#define L_MOTOR 0 /\* motor port 0 \*/**

**void main()**

**{**

**printf(\"Simple forward -back test r1\\n\");**

**start_press();**

**simple_straight(1);**

**sleep(1.);**

**ao();sleep(.2);**

**simple_straight(-1);**

**sleep(1.);**

**ao();**

**printf(\"Done\\n\");**

**}**

**void simple_straight(int dir)**

**{**

**int l_speed=SPEED\*dir, r_speed=(SPEED + DELTA)\*dir;**

**motor( L_MOTOR, l_speed/3);**

**motor(R_MOTOR, r_speed/3);**

**sleep(.2);**

**motor( L_MOTOR, l_speed);**

**motor(R_MOTOR, r_speed);**

**}**

**/\* simple forward-back test2 - gradual start - repeat Grant 2/5/05
\*/**

**#define DELTA 0 /\* direction adjustment: + = more turn to left, - =
more to right \*/**

**#define SPEED 75 /\* nominal speed \*/**

**/\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\* Robot port configuration
\*\*\*\*\*\*\*\*\*\*\*/**

**#define R_MOTOR 2 /\* motor port 2 \*/**

**#define L_MOTOR 0 /\* motor port 0 \*/**

**void main()**

**{**

**printf(\"Simple forward -back test r2\\n\");**

**while(!stop_button()){**

**start_press();**

**simple_straight(1);**

**sleep(1.);**

**ao();sleep(.2);**

**simple_straight(-1);**

**sleep(1.);**

**ao();**

**}**

**printf(\"Done\\n\");**

**}**

**void simple_straight(int dir)**

**{**

**int l_speed=SPEED\*dir, r_speed=(SPEED + DELTA)\*dir;**

**motor( L_MOTOR, l_speed/3);**

**motor(R_MOTOR, r_speed/3);**

**sleep(.2);**

**motor( L_MOTOR, l_speed);**

**motor(R_MOTOR, r_speed);**

**}**

**/\* simple forward-back test3 - gradual start -- repeat + brake Grant
2/5/05 \*/**

**#define DELTA 0 /\* direction adjustment: + = more turn to left, - =
more to right \*/**

**#define SPEED 75 /\* nominal speed \*/**

**/\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\*\* Robot port configuration
\*\*\*\*\*\*\*\*\*\*\*/**

**#define R_MOTOR 2 /\* motor port 2 \*/**

**#define L_MOTOR 0 /\* motor port 0 \*/**

**void main()**

**{**

**printf(\"Simple forward -back test r3\\n\");**

**while(!stop_button()){**

**start_press();**

**simple_straight(1);**

**sleep(1.);**

**stop_wheels(1);**

**sleep(.2);**

**simple_straight(-1);**

**sleep(1.);**

**stop_wheels(-1);**

**}**

**printf(\"Done\\n\");**

**}**

**void simple_straight(int dir)**

**{**

**int l_speed=SPEED\*dir, r_speed=(SPEED + DELTA)\*dir;**

**motor( L_MOTOR, l_speed/3);**

**motor(R_MOTOR, r_speed/3);**

**sleep(.2);**

**motor( L_MOTOR, l_speed);**

**motor(R_MOTOR, r_speed);**

**}**

**/\* reverses motors briefly for a quick stop \*/**

**void stop_wheels(int pol)**

**{**

**motor( L_MOTOR,-20\*pol);**

**motor(R_MOTOR,-20\*pol);**

**sleep(0.05);**

**ao();**

**}**

**/\* bang-bang control to avoid obstacles using rangefinders - Grant
2/5/05\*/**

**#define DELTA 0 /\* extra speed for right motor \*/**

**#define R_MOTOR 2 /\* motor port 2 \*/**

**#define L_MOTOR 0 /\* motor port 0 \*/**

**#define L_RANGE 18 /\* range sensor in analog 18\*/**

**#define R_RANGE 16 /\* range sensor in analog 16\*/**

**#define SPEED 55 /\* PWM % for motors \*/**

**#define THROW_DIST 195 /\* sensor reading to throw the ball, avoid
obstacles, etc\*/**

**#define T 5000L /\* run time in millisec \*/**

**int L_Range, R_Range; /\* *globals for left & right sensors* \*/**

**void main()**

**{**

**start_process(monitor_sensors());**

**printf(\"range avoid press start\\n\");**

**start_press(); sleep(3.);/\* wait for start button press \*/**

**avoid();**

**}**

**void avoid()**

**{**

**int l_speed, r_speed;**

**long time_s=mseconds()+T;**

**while(!stop_button()&&(mseconds()\<time_s))**

**{**

**l_speed=SPEED; r_speed=SPEED+DELTA;**

**if(L_Range\<=THROW_DIST) r_speed=0;**

**else {**

**if(R_Range\<=THROW_DIST) l_speed=0;**

**}**

**motor( L_MOTOR, l_speed);**

**motor(R_MOTOR, r_speed);**

**defer();**

**}**

**ao();**

**}**

**void monitor_sensors()//range sensors only**

**{**

**while(1){**

**L_Range= 255-analog(L_RANGE); /\* range reading is big for big
distances \*/**

**R_Range = 255-analog(R_RANGE);**

**defer();**

**}**

**}**

**// Go Throw simple version tlg 2/8/05**

**#define ARM 3**

**#define THROW_DIST 195**

**#define L_MOTOR 0**

**#define R_MOTOR 2**

**#define DELTA 0**

**#define L_RANGE 18 /\* range sensor in analog 18\*/**

**#define R_RANGE 16**

**int L_Range,R_Range;**

**void main()**

**{**

**start_process(monitor_sensors());**

**printf(\"Go Throw Press START\\n\");**

**while(!start_button());**

**go_throw();**

**}**

**/\* uses p-loop on speed relative to distance \*/**

**void go_throw()**

**{**

**int range =(R_Range+L_Range)/2;**

**beep();**

**sleep(1.);**

**reset_encoder(0); reset_encoder(1);**

**defer();**

**while((range\>THROW_DIST)&&!stop_button()) {**

**range=(R_Range+L_Range)/2;**

**go_straight(range-THROW_DIST+15);**

**defer();**

**}**

**throw_ball();**

**}**

**/\* jerks the arm forward briefly and turns all motors off \*/**

**void throw_ball()**

**{**

**motor(ARM, 100);**

**sleep(0.4);**

**ao();**

**}**

**/\* open loop \[no control\] for the robot heading straight except
DELTA speed on right motor \*/**

**void go_straight(int speed)//simplified to not use encoders**

**{**

**int l_speed, r_speed;**

**l_speed=speed; r_speed=speed+DELTA;**

**motor( L_MOTOR, l_speed);**

**motor(R_MOTOR, r_speed);**

**}**

**//Go Throw simple 2/8/05 continued**

**void monitor_sensors()//range sensors only**

**{**

**while(1){**

**L_Range= 255-analog(L_RANGE); /\* range reading is big for big**

**distances \*/**

**R_Range = 255-analog(R_RANGE);**

**defer();**

**}**

**}**