int check_stopping_event(int stopping_event) {
if (stopping_event == STOP_BY_ET) {
if (analog_et(ET_SENSOR) < 300) {
return 1;
}
} else if (stopping_event == STOP_BY_TOPHAT) {
display_printf(0, 0, "%4i", analog(L_TOPHAT));
if (analog(L_TOPHAT) > 750 || analog(R_TOPHAT) > 750) {
return 1;
}
} else if (stopping_event == STOP_BY_CAMERA) {
camera_update();
if (get_object_count(0) < 1) {
display_printf(0, 0, "No Object Found");
return 0;
}
if (get_object_area(0, 0) > PINGPONG_THRESHOLD) {
display_clear();
display_printf(0, 0, "Object Seen!");
if (get_object_center(0, 0).x > 90 && get_object_center(0, 0).x < 110) {
display_printf(0, 1, "Object centered");
return 1;
}
}
} else {
printf("Stopping type is not defined!\n");
return -1;
}
return 0;
}
void wall_follow(int direction, int desired_speed, int stopping_event) {
int l_speed, r_speed, sensor_reading, error;
while (1) {
sensor_reading = analog_et(ET_SENSOR);
error = sensor_reading - ET_DESIRED_VALUE;
l_speed = desired_speed - (error * ET_kP);
if (l_speed > desired_speed + ET_MAXIMUM_SPEED_DEVIATION) {
l_speed = desired_speed + ET_MAXIMUM_SPEED_DEVIATION;
} else if (l_speed < desired_speed - ET_MAXIMUM_SPEED_DEVIATION) {
l_speed = desired_speed - ET_MAXIMUM_SPEED_DEVIATION;
}
r_speed = desired_speed + (error * ET_kP);
if (r_speed > desired_speed + ET_MAXIMUM_SPEED_DEVIATION) {
r_speed = desired_speed + ET_MAXIMUM_SPEED_DEVIATION;
} else if (r_speed < desired_speed - ET_MAXIMUM_SPEED_DEVIATION) {
r_speed = desired_speed - ET_MAXIMUM_SPEED_DEVIATION;
}
motor(LEFT_MOTOR, ((int) l_speed) * direction);
motor(RIGHT_MOTOR, ((int) r_speed) * direction);
if (check_stopping_event(stopping_event) == 1) {
off(LEFT_MOTOR);
off(RIGHT_MOTOR);
break;
}
}
}
void lego_drive_until_opening(int direction, int speed) {
int left_ticks_traveled = 0;
int right_ticks_traveled = 0;
float right_error;
float left_error;
float desired_value;
int left_multiplier;
int right_multiplier;
int left_sensor_value;
int right_sensor_value;
clear_motor_position_counter(LEFT_MOTOR);
clear_motor_position_counter(RIGHT_MOTOR);
motor(LEFT_MOTOR, speed * direction);
motor(RIGHT_MOTOR, speed * direction);
while(TRUE) {
left_sensor_value = analog_et(LEFT_ET);
right_sensor_value = analog_et(RIGHT_ET);
if (left_sensor_value < ET_NO_WALL && right_sensor_value < ET_NO_WALL) {
break;
}
left_ticks_traveled = get_motor_position_counter(LEFT_MOTOR);
right_ticks_traveled = get_motor_position_counter(RIGHT_MOTOR);
desired_value = (left_ticks_traveled + right_ticks_traveled) / 2.0;
left_error = desired_value - left_ticks_traveled;
right_error = desired_value - right_ticks_traveled;
left_multiplier = (int) ((left_error * l_kP) + 0.5);
right_multiplier = (int) ((right_error * r_kP) + 0.5);
motor(LEFT_MOTOR, (speed * direction) + left_multiplier);
motor(RIGHT_MOTOR, (speed * direction) + right_multiplier);
}
freeze(LEFT_MOTOR);
freeze(RIGHT_MOTOR);
}
void moveFor(){
int frontDis = analog_et(FSS);
int backDis = analog_et(BSS);
int disDiff = frontDis - backDis;
int turnLM = MOVE_FOR_TP - disDiff * KDp;
int turnRM = MOVE_FOR_TP + disDiff * KDp;
while(frontDis > 295){
motor(LM, turnLM);
motor(RM, turnRM);
frontDis = analog_et(FSS);
backDis = analog_et(BSS);
printf("%d\t%d\n", frontDis, backDis);
disDiff = frontDis - backDis;
turnLM = MOVE_BACK_TP - disDiff * KDp;
turnRM = MOVE_BACK_TP + disDiff * KDp;
}
clear_motor_position_counter(LM);
clear_motor_position_counter(RM);
mtp(LM, 800, 500);
mtp(RM, 800, 500);
bmd(LM);
bmd(RM);
clear_motor_position_counter(LM);
clear_motor_position_counter(RM);
mtp(LM, 800, 260);
mtp(RM, -800, -260);
bmd(LM);
bmd(RM);
while(analog(RSS) < RSS_OFFSET){
motor(RM, 82);
motor(LM, 90);
}
while(analog(RSS) > RSS_OFFSET){
motor(RM, 82);
motor(LM, 90);
}
while(analog(RSS) < RSS_OFFSET){
motor(RM, 82);
motor(LM, 90);
}
}
void moveBack1(){
int frontDis = analog_et(FSS);
int backDis = analog_et(BSS);
int errorFront = FSS_BACK_DIS - frontDis;
int errorBack = BSS_BACK_DIS - backDis;
int turnLM = MOVE_BACK_TP + errorFront * FSS_BACK_Kp + errorBack * BSS_BACK_Kp;
int turnRM = MOVE_BACK_TP - errorFront * FSS_BACK_Kp - errorBack * BSS_BACK_Kp;
while(digital(TSS) == 0 && backDis > 280){
motor(LM, turnLM);
motor(RM, turnRM - 4);
frontDis = analog_et(FSS);
backDis = analog_et(BSS);
errorFront = FSS_BACK_DIS - frontDis;
errorBack = BSS_BACK_DIS - backDis;
turnLM = MOVE_BACK_TP - errorFront * FSS_BACK_Kp / 1000 + errorBack * BSS_BACK_Kp / 1000;
turnRM = MOVE_BACK_TP + errorFront * FSS_BACK_Kp / 1000 - errorBack * BSS_BACK_Kp / 1000;
}
msleep(150);
while(digital(TSS) == 0 && frontDis > 295){
frontDis = analog_et(FSS);
errorFront = FSS_BACK_DIS - frontDis;
turnLM = MOVE_BACK_TP - errorFront * FSS_BACK_Kp / 1000;
turnRM = MOVE_BACK_TP + errorFront * FSS_BACK_Kp / 1000;
motor(LM, turnLM);
motor(RM, turnRM - 2);
msleep(50);
}
while(analog(RSS) < RSS_OFFSET){
motor(RM, -82);
motor(LM, -90);
}
while(analog(RSS) > RSS_OFFSET){
motor(RM, -82);
motor(LM, -90);
}
while(analog(RSS) < RSS_OFFSET){
motor(RM, -82);
motor(LM, -90);
}
}
void moveBack2(){
int frontDis = analog_et(FSS);
int backDis = analog_et(BSS);
int disDiff = frontDis - backDis;
int turnLM = MOVE_BACK_TP - disDiff * KDp;
int turnRM = MOVE_BACK_TP + disDiff * KDp;
double timeStart = seconds();
double timeEnd = seconds();
while((timeEnd - timeStart) < 1.5){
motor(LM, turnLM);
motor(RM, turnRM);
frontDis = analog_et(FSS);
backDis = analog_et(BSS);
printf("%d\t%d\n", frontDis, backDis);
disDiff = frontDis - backDis;
turnLM = MOVE_BACK_TP - disDiff * KDp;
turnRM = MOVE_BACK_TP + disDiff * KDp;
timeEnd = seconds();
}
}
int et_wall_stop() {
if (analog_et(LEFT_ET) > ET_WALL && analog_et(RIGHT_ET) > ET_WALL) {
return TRUE;
}
return FALSE;
}
void lego_spin_onto_black_line(int direction, int speed, int low_threshold, int high_threshold) {
int left_ticks_traveled = 0;
int right_ticks_traveled = 0;
float right_error;
float left_error;
float desired_value;
int left_multiplier;
int right_multiplier;
int i;
clear_motor_position_counter(LEFT_MOTOR);
clear_motor_position_counter(RIGHT_MOTOR);
motor(LEFT_MOTOR, speed * direction);
motor(RIGHT_MOTOR, -speed * direction);
for (i = 0; i < 3; i++) {
while(TRUE) {
if (analog_et(RIGHT_ET) >= low_threshold) {
break;
}
left_ticks_traveled = abs(get_motor_position_counter(LEFT_MOTOR));
right_ticks_traveled = abs(get_motor_position_counter(RIGHT_MOTOR));
desired_value = (left_ticks_traveled + right_ticks_traveled) / 2.0;
left_error = desired_value - left_ticks_traveled;
right_error = desired_value - right_ticks_traveled;
left_multiplier = (int) ((left_error * spin_l_kP) + 0.5);
right_multiplier = (int) ((right_error * spin_r_kP) + 0.5);
motor(LEFT_MOTOR, (speed * direction) + left_multiplier);
motor(RIGHT_MOTOR, (-speed * direction) + right_multiplier);
display_printf(0, 0, "L: %4i", ((speed * direction) + left_multiplier));
display_printf(0, 1, "R: %4i", ((speed * direction) + right_multiplier));
}
msleep(10);
while(TRUE) {
if (analog_et(RIGHT_ET) <= high_threshold) {
break;
}
left_ticks_traveled = abs(get_motor_position_counter(LEFT_MOTOR));
right_ticks_traveled = abs(get_motor_position_counter(RIGHT_MOTOR));
desired_value = (left_ticks_traveled + right_ticks_traveled) / 2.0;
left_error = desired_value - left_ticks_traveled;
right_error = desired_value - right_ticks_traveled;
left_multiplier = (int) ((left_error * spin_l_kP) + 0.5);
right_multiplier = (int) ((right_error * spin_r_kP) + 0.5);
motor(LEFT_MOTOR, (-speed * direction) + left_multiplier);
motor(RIGHT_MOTOR, (speed * direction) + right_multiplier);
display_printf(0, 0, "L: %4i", ((speed * direction) + left_multiplier));
display_printf(0, 1, "R: %4i", ((speed * direction) + right_multiplier));
}
}
freeze(LEFT_MOTOR);
freeze(RIGHT_MOTOR);
}
