#include <conio.h>
#include <string.h>
#include <unistd.h>
#include <dsensor.h>
#include <stdlib.h>
#include <unistd.h>
#include <dmotor.h>
#include <dsound.h>
#include <lnp.h>
#define TH_WHITE 0x2E
#define TH_BLACK 0x2A
#define LIGHT_SENSOR SENSOR_3
#define CHECK_WHITE (LIGHT(LIGHT_SENSOR)>=TH_WHITE)
#define CHECK_BLACK (LIGHT(LIGHT_SENSOR)<=TH_BLACK)
#define ARM ROTATION_1
#define ARM_SENSOR SENSOR_1
#define ARM_ORTOG 20
#define ARM_MAX 35
#define ARM_PREV 32
#define ROAD_WIDTH 7
struct s_road {
short left;
short right;
short error;
};
#define ABS(x) ( x>=0 ? (x) : (-x) )
void arm_to(int x) {
while (ARM!=x) {
while (ARM!=x) {
if (ARM>x)
motor_b_dir(fwd);
else
motor_b_dir(rev);
}
motor_b_dir(brake);
delay(50);
}
}
void arm_calibr() {
int armright, armleft;
motor_b_speed(60);
motor_b_dir(fwd);
armright=ARM;
delay(50);
while (ARM!=armright) {
armright=ARM;
delay(50);
}
motor_b_dir(brake);
motor_b_dir(rev);
armleft=ARM;
delay(50);
while (ARM!=armleft) {
armleft=ARM;
delay(50);
}
motor_b_dir(brake);
motor_b_speed(40);
arm_to((armright+armleft+1)/2);
ds_rotation_set(&ARM_SENSOR, 0);
delay(100);
}
void init() {
motor_a_dir(off);
motor_b_dir(off);
motor_c_dir(off);
motor_a_speed(255);
motor_b_speed(30);
motor_c_speed(255);
ds_active(&LIGHT_SENSOR);
ds_active(&ARM_SENSOR);
ds_rotation_on(&ARM_SENSOR);
ds_rotation_set(&ARM_SENSOR, 0);
arm_calibr();
}
void deinit() {
motor_b_dir(off);
motor_a_dir(off);
motor_c_dir(off);
ds_passive(&LIGHT_SENSOR);
ds_rotation_off(&ARM_SENSOR);
ds_passive(&ARM_SENSOR);
exit(0);
}
int fence_search(short* ret) {
int arm=ARM;
while (CHECK_BLACK && ABS(ARM-arm)<=ARM_ORTOG/2);
motor_b_dir(brake);
*ret=ARM;
if (ABS(ARM-arm)>=ARM_ORTOG/2)
return 0;
else
return 1;
}
void _calibr(int x, int adir, int cdir) {
motor_a_dir(adir);
motor_c_dir(cdir);
delay(x);
motor_a_dir(brake);
motor_c_dir(brake);
}
#define AHEAD_CALIBR(x) _calibr(x, fwd, fwd)
#define RIGHT_CALIBR(x) _calibr(x, fwd, rev)
#define LEFT_CALIBR(x) _calibr(x, rev, fwd)
#define BACK_CALIBR(x) _calibr(x, rev, rev)
int arm_max(int x) {
int cb=0;
while (ARM!=x && !(cb=CHECK_BLACK)) {
if (ARM>x)
motor_b_dir(fwd);
else
motor_b_dir(rev);
}
motor_b_dir(brake);
return cb;
}
struct s_road road_search() {
struct s_road ret;
arm_to(0);
if (!CHECK_BLACK) {
int road_left=0,road_right=0;
// the road is not ahead, trouble, trouble
// it can be on the left
arm_to(ARM_ORTOG/8);
road_left=arm_max(ARM_ORTOG/2);
delay(50);
// or on the right
arm_to(-ARM_ORTOG/8);
road_right=arm_max(-ARM_ORTOG/2);
arm_to(0);
// but can't be both
if (road_left && road_right) {
cputs("ER1");
deinit();
} else if (road_left) {
arm_to(-2);
while (!CHECK_BLACK) { LEFT_CALIBR(25); delay(50); }
}
else if (road_right) {
arm_to(2);
while (!CHECK_BLACK) { RIGHT_CALIBR(25); delay(50); }
}
else {
ret.error=1;
ret.right=0;
ret.left=0;
return ret;
}
}
arm_to(0);
motor_b_dir(fwd);
if (!fence_search(&ret.right)) {
ret.error=1;
return ret;
}
motor_b_dir(rev);
while(ARM<0);
if (!fence_search(&ret.left)) {
ret.error=1;
return ret;
}
ret.error=0;
return ret;
}
void calibr_after_turn() {
int i=0;
arm_to(0);
while (!CHECK_BLACK && i++<=2) {LEFT_CALIBR(25); delay(50);}
while (!CHECK_WHITE) {LEFT_CALIBR(25); delay(50);}
while (!CHECK_BLACK) {RIGHT_CALIBR(25); delay(50);}
}
void right_turn() {
cputs("RIGH");
arm_to(-ARM_ORTOG);
AHEAD_CALIBR(10);
delay(10);
while(!CHECK_BLACK) {
AHEAD_CALIBR(20);
delay(25);
}
arm_to(-ARM_ORTOG);
while (!CHECK_WHITE) {RIGHT_CALIBR(40); delay(50);}
while (!CHECK_BLACK) {RIGHT_CALIBR(40); delay(50);}
calibr_after_turn();
RIGHT_CALIBR(40);
}
void left_turn() {
cputs("LEFT");
arm_to(ARM_ORTOG-3);
AHEAD_CALIBR(10);
delay(10);
while(!CHECK_BLACK) {
AHEAD_CALIBR(20);
delay(25);
}
arm_to(ARM_ORTOG);
while (!CHECK_WHITE) {LEFT_CALIBR(40); delay(50);}
while (!CHECK_BLACK) {LEFT_CALIBR(40); delay(50);}
calibr_after_turn();
BACK_CALIBR(40);
}
void no_turn() {
cputs("AHEA");
calibr_after_turn();
}
void around_turn() {
cputs("TURN");
arm_to(ARM_MAX);
while (ARM>0) {
// spin until white occurs after black
while (!CHECK_BLACK) { LEFT_CALIBR(40); delay(20); }
while (CHECK_BLACK) { LEFT_CALIBR(40); delay(20); }
while (!CHECK_WHITE) { LEFT_CALIBR(40); delay(20); }
// turn the arm back
arm_to(ARM-10);
BACK_CALIBR(12);
delay(20);
}
BACK_CALIBR(20);
calibr_after_turn();
}
static char* junction_search(int havetodetermine) {
struct s_road road_res;
int white_left=0;
int white_right=0;
static char ret[5];
cputs("SEAR");
ret[4]=0;
road_res=road_search();
while (road_res.error==0 && road_res.left-road_res.right<=ROAD_WIDTH) {
if (ABS((short)(road_res.left+road_res.right))<=1)
AHEAD_CALIBR(20);
else if (((short)road_res.left+road_res.right)<0)
RIGHT_CALIBR(15);
else
LEFT_CALIBR(15);
road_res=road_search();
// cputw(ROAD_DIFF(road_res));
}
motor_a_dir(brake);
motor_c_dir(brake);
cputs("FIND");
if (havetodetermine) {
arm_to(-ARM_ORTOG);
if (arm_max(-ARM_PREV))
ret[3]='1';
else {
arm_to(ARM_ORTOG);
if (arm_max(ARM_PREV))
ret[3]='1';
else
ret[3]='2';
}
}
AHEAD_CALIBR(30);
while (!white_left || !white_right) {
AHEAD_CALIBR(20);
arm_to(-ARM_ORTOG/2);
white_right=CHECK_WHITE;
arm_to(ARM_ORTOG/2);
white_left=CHECK_WHITE;
}
if (havetodetermine) {
cputs("DETR");
arm_to(ARM_ORTOG);
ret[0]=arm_max(ARM_ORTOG/2)+48;
arm_to(ARM_ORTOG/4);
ret[1]=arm_max(-ARM_ORTOG/4)+48;
arm_to(-ARM_ORTOG/2);
ret[2]=arm_max(-ARM_ORTOG)+48;
return ret;
} else return "NRES";
}
#ifndef ROBOTOOLS_INCLUDED
int main(int argc, char **argv) {
init();
cputs(junction_search(1));
delay(400);
right_turn();
cputs(junction_search(1));
deinit();
return 0;
}
#endif