//right angle turn function
void rightAngleBwdMrp(int direction, int debug ) {
mav(RIGHT_MOTOR, 0);
mav(LEFT_MOTOR, 0);
if (debug == DEBUG) {
printf("starting right angle mrp bwd \n");
}
msleep(100);
int initial_position = get_motor_position_counter(RIGHT_MOTOR);
if (direction == RIGHT) {
initial_position = get_motor_position_counter(LEFT_MOTOR);
}
int current_position = initial_position;
if (direction == RIGHT) {
mrp(LEFT_MOTOR, SPEED_BWD, RIGHT_ANGLE_CLICKS_BACK_MRP);
bmd(LEFT_MOTOR);
current_position = get_motor_position_counter(LEFT_MOTOR);
} else if (direction == LEFT) {
mrp(RIGHT_MOTOR, SPEED_BWD, RIGHT_ANGLE_CLICKS_BACK_MRP);
bmd(RIGHT_MOTOR);
current_position = get_motor_position_counter(RIGHT_MOTOR);
}
if (debug == DEBUG) {
printf("right angle mrp bwd Init %d , curr %d, tgt %d\n", initial_position,current_position, RIGHT_ANGLE_CLICKS_BACK_MRP);
}
mav(RIGHT_MOTOR, 0);
mav(LEFT_MOTOR, 0);
reset_motors();
}
//right angle turn function
void rightAngleFwdMrp(int direction, int debug ) {
mav(RIGHT_MOTOR, 0);
mav(LEFT_MOTOR, 0);
msleep(100);
int initial_position = get_motor_position_counter(RIGHT_MOTOR);
if (direction == RIGHT) {
initial_position = get_motor_position_counter(LEFT_MOTOR);
}
int current_position = initial_position;
if (direction == RIGHT) {
mrp(LEFT_MOTOR, SPEED_FWD, RIGHT_ANGLE_CLICKS_MRP_RIGHT);
bmd(LEFT_MOTOR);
if (debug == DEBUG) {
printf("right angle mrp fwd RIGHT Init %d , curr %d, tgt %d\n", initial_position,current_position, RIGHT_ANGLE_CLICKS_MRP_RIGHT);
}
current_position = get_motor_position_counter(LEFT_MOTOR);
} else if (direction == LEFT) {
mrp(RIGHT_MOTOR, SPEED_FWD, RIGHT_ANGLE_CLICKS_MRP_LEFT);
bmd(RIGHT_MOTOR);
if (debug == DEBUG) {
printf("right angle mrp fwd LEFT Init %d , curr %d, tgt %d\n", initial_position,current_position, RIGHT_ANGLE_CLICKS_MRP_LEFT);
}
current_position = get_motor_position_counter(RIGHT_MOTOR);
}
mav(RIGHT_MOTOR, 0);
mav(LEFT_MOTOR, 0);
reset_motors();
}
//right angle turn function
void rightAngleBwdMav(int direction, int debug ) {
mav(RIGHT_MOTOR, 0);
mav(LEFT_MOTOR, 0);
msleep(100);
int clickNbrTarget = RIGHT_ANGLE_CLICKS_BACK;
int initial_position = get_motor_position_counter(RIGHT_MOTOR);
if (direction == RIGHT) {
clickNbrTarget = RIGHT_ANGLE_CLICKS_BACK;
initial_position = get_motor_position_counter(LEFT_MOTOR);
}
int current_position = initial_position;
if (direction == RIGHT) {
mav(LEFT_MOTOR,SPEED_BWD);
} else if (direction == LEFT) {
mav(RIGHT_MOTOR, SPEED_BWD) ;
}
while ((current_position - initial_position) >= clickNbrTarget) {
msleep(25);
current_position = get_motor_position_counter(RIGHT_MOTOR);
if (direction == RIGHT) {
current_position = get_motor_position_counter(LEFT_MOTOR);
}
if (debug == DEBUG) {
printf("right angle bwd mav Init %d , curr %d, tgt %d\n", initial_position,current_position , clickNbrTarget );
}
}
mav(RIGHT_MOTOR, 0);
mav(LEFT_MOTOR, 0);
reset_motors();
}
void moveBackwardRoutine(double distanceInInches,int speed, int debug) {
//convert inches to clicks
int clicks =(int) (156.25l * distanceInInches);
int initial_position_right = get_motor_position_counter(RIGHT_MOTOR);
int initial_position_left = get_motor_position_counter(LEFT_MOTOR);
int current_position_right = get_motor_position_counter(RIGHT_MOTOR);
int current_position_left = get_motor_position_counter(LEFT_MOTOR);
int differential = 0 ;
while (current_position_left >= (initial_position_left - clicks) ||
current_position_right >= (initial_position_right - clicks) ) {
//first let's see if one motor is going ahead of the other
differential = current_position_left - initial_position_left -
(current_position_right - initial_position_right);
if (differential > -25 && differential < 25 ) {
mav(RIGHT_MOTOR, speed);
mav(LEFT_MOTOR, speed);
} else if (differential > 0 ) {
mav(RIGHT_MOTOR, (int) (speed*ADJUST_SPEED));
mav(LEFT_MOTOR, (speed * 1.1));
} else {
mav(RIGHT_MOTOR, (speed * 1.1));
mav(LEFT_MOTOR, (int) (speed*ADJUST_SPEED));
}
msleep(25);
current_position_right = get_motor_position_counter(RIGHT_MOTOR);
current_position_left = get_motor_position_counter(LEFT_MOTOR);
}
//turn off motors completely
mav(RIGHT_MOTOR, 0);
mav(LEFT_MOTOR,0);
ao();
reset_motors();
}
static int perform_ground_actions( void )
{
if( motor_signals.front > 0 || motor_signals.right > 0 ||
motor_signals.rear > 0 || motor_signals.left > 0 )
{
if( motor_signals.front > 0 )
{
motor_signals.front -= MOTOR_REVVING_STEP;
}
if( motor_signals.right > 0 )
{
motor_signals.right -= MOTOR_REVVING_STEP;
}
if( motor_signals.rear > 0 )
{
motor_signals.rear -= MOTOR_REVVING_STEP;
}
if( motor_signals.left > 0 )
{
motor_signals.left -= MOTOR_REVVING_STEP;
}
}
else
{
reset_motors( );
reset_controllers( );
}
return( 0 );
}
int main(int argc,char** argv){
int found = 0, arrived_home = 0;
int use_http = 0;
char address[200];
int port = 55555;
strcpy(address, "localhost");
if(argc >= 4){
use_http = atoi(argv[1]);
strncpy(address, argv[2], 200);
port = atoi(argv[3]);
}else{
printf("[INFO] Usage: 'prog 1 address port' to start it with http connection\n");
}
//initalization
init(use_http, address, port);
//install signal handler to exit gently
signal(SIGINT,sigint_handler);
//scheduling
while(!end_game && !arrived_home){
if(robot_rank == 0){
// Leader mode
if(found == 0){
found = leader();
}else{
arrived_home = go_back_home();
send_lead(next);
}
}else{
// Follower mode
while(robot_rank > 0 && !end_game){
follower();
}//######################aggiunger end game e stop anche nel leader e go back home etc
//follower call back called at each action messagge
}
}
if(end_game){
printf("[STATUS] Game ended\n");
}else{
printf("[STATUS] Back home\n");
}
reset_motors();
destroy_sensors();
destroy_motors();
destroy_bluetooth();
free(positions);
}
void twentyTwoAngleFwd(int direction, int debug) {
//twenty two .5 degree backwards turn
if (direction == RIGHT) {
mrp(LEFT_MOTOR,(int)(SPEED_FWD*.7),TT_ANGLE_CLICKS);
bmd(LEFT_MOTOR);
} else if (direction == LEFT) {
mrp(RIGHT_MOTOR, (int)(SPEED_FWD*.7), TT_ANGLE_CLICKS) ;
bmd(RIGHT_MOTOR);
} else {
printf("ooopppsss I did not recognize your turn... so I ignored it");
}
//turn off motors completely
mav(RIGHT_MOTOR, 0);
mav(LEFT_MOTOR,0);
reset_motors();
}
void fortyFiveAngleFwd(int direction, int debug) {
//forty five degree backwards turn
if (direction == RIGHT) {
mrp(LEFT_MOTOR,(SPEED_FWD),FV_ANGLE_CLICKS);
bmd(LEFT_MOTOR);
} else if (direction == LEFT) {
mrp(RIGHT_MOTOR, (SPEED_FWD), FV_ANGLE_CLICKS) ;
bmd(RIGHT_MOTOR);
} else {
printf("ooopppsss I did not recognize your turn... so I ignored it");
}
//turn off motors completely
mav(RIGHT_MOTOR, 0);
mav(LEFT_MOTOR,0);
reset_motors();
}
//moves forward with light sensor
void moveForwardRoutine(double distanceInInches, int checkIRSensor, int speed, int debug) {
//checkLightSensor do not check light sensor: see #define values
// do check light sensor and stop when it is over black or void
//convert inches to clicks
int speed_adjust_up = (int) (speed / ADJUST_SPEED);
int speed_adjust_down = (int) (speed * ADJUST_SPEED);
printf("speed down %d, speed up: %d\n", speed_adjust_down, speed_adjust_up);
int clicks =(int) (156.25l * distanceInInches);
int initial_position_right = get_motor_position_counter(RIGHT_MOTOR);
int initial_position_left = get_motor_position_counter(LEFT_MOTOR);
int current_position_right = get_motor_position_counter(RIGHT_MOTOR);
int current_position_left = get_motor_position_counter(LEFT_MOTOR);
float differential = 0 ;
//we will keep moving until both motors have covered their distance or
//the light sensor is tripped
while (current_position_left <= (initial_position_left + clicks) ||
current_position_right <= (initial_position_right + clicks) ) {
//let's get out if the sensor is on
if (checkIRSensor == CHECK_IR_SENSOR && analog(IR_SENSOR) > IR_SENSOR_THRESHOLD) {
if (debug == DEBUG) {
printf("exiting because of light sensor");
}
break;
}
//first let's see if one motor is going ahead of the other
differential = (current_position_left - initial_position_left)
* MOTOR_CORRECTION_FACTOR_LEFT ;
differential = differential - (current_position_right - initial_position_right);
// if (debug == DEBUG) {
// printf("sensor value: %d lightsensor %d \n", analog(IR_SENSOR),checkIRSensor);
// }
if (differential > -25 && differential < 25 ) {
//counter are around the same
mav(RIGHT_MOTOR, speed);
mav(LEFT_MOTOR, speed);
} else if (differential < 0 ) {
//right has moved ahead, let's slow down right until left catches up
mav(RIGHT_MOTOR,speed_adjust_down);
mav(LEFT_MOTOR, speed_adjust_up);
if (debug == DEBUG) {
printf("adjusting LEFT L: %d R: %d, diff %f\n", (current_position_left - initial_position_left), (current_position_right - initial_position_right), differential);
}
} else {
//left has moved ahead, let's slow down left until right catches up
mav(RIGHT_MOTOR, speed_adjust_up);
mav(LEFT_MOTOR, speed_adjust_down);
if (debug == DEBUG) {
printf("adjusting RIGHT L: %d R: %d diff %f\n", (current_position_left - initial_position_left), (current_position_right - initial_position_right), differential);
}
}
msleep(25);
current_position_right = get_motor_position_counter(RIGHT_MOTOR);
current_position_left = get_motor_position_counter(LEFT_MOTOR);
}
//turn off motors completely
mav(RIGHT_MOTOR, 0);
mav(LEFT_MOTOR,0);
ao();
reset_motors();
}
static void perform_shut_down( void )
{
reset_motors( );
reset_filters( );
reset_controllers( );
}
