void blockFarEntrance() {
CardboardMid();
set_servo_position(2, 800);
msleep(200);
LinkDrive(2, 1600);
set_servo_position(2, 1500);
msleep(100);
mav(LEFT_PORT, 1600*LINK_LEFT_OFFSET);
msleep(200);
set_servo_position(2, 800);
LinkDrive(3, 1600);
set_servo_position(2, 1500);
msleep(100);
set_servo_position(2, 800);
LinkDrive(3, 1600);
set_servo_position(2, 1500);
LinkDrive(18, 1600);
CardboardMid();
mav(RIGHT_PORT, 1600*LINK_RIGHT_OFFSET);
mav(LEFT_PORT, -1600*LINK_LEFT_OFFSET);
msleep(200);
LinkDrive(8,1600);
LinkThrowIgus();
msleep(1750);
CardboardHi();
// mav(RIGHT_PORT, 1600*LINK_RIGHT_OFFSET);
// msleep(70);
}
inline int camera_move_x()
{
int lspeed = -100;
int hspeed = 100;
blob_update();
printf("MOVING X\n");
if (current.green.x < (target.green.x + TOL))
{
printf("LEFT\n");
mav(lego.left.port , lspeed);
mav(lego.right.port , hspeed);
return 1;
}
if (current.green.x > (target.green.x - TOL))
{
printf("RIGHT\n");
mav(lego.left.port , hspeed);
mav(lego.right.port , lspeed);
return 1;
}
if (current.green.x >= (target.green.x - TOL) && current.green.x <= (target.green.x + TOL))
{
ao();
return 0;
}
}
~Sling() {
arm.move_to( iPair(0,0) );
mav(motor_shoulder, -200);
mav(motor_arm, -200);
sleep(0.2);
ao();
}
void Forty_Five_Degree_AngleMAV(int Direction, double timeLimit){
long rightClicksRight=0.35*TICKS_PER_REVOLUTION_RIGHT; //39
long leftClicksRight=0.35*TICKS_PER_REVOLUTION_LEFT;
clear_motor_position_counter(LEFT_PORT_WHEEL);
clear_motor_position_counter(RIGHT_PORT_WHEEL);
if(Direction==RIGHT){
double startTURN45Time = seconds();
while((seconds() - startTURN45Time <= timeLimit) && ((get_motor_position_counter(LEFT_PORT_WHEEL) < leftClicksRight) && (get_motor_position_counter(RIGHT_PORT_WHEEL) > -rightClicksRight))){
mav(RIGHT_PORT_WHEEL, -CRUISING_SPEED_TURNS);
mav(LEFT_PORT_WHEEL, CRUISING_SPEED_TURNS);
msleep(35);
}
}
else if(Direction==LEFT){
double startTURN45Time = seconds();
while((seconds() - startTURN45Time <= timeLimit) && ((get_motor_position_counter(RIGHT_PORT_WHEEL) < rightClicksRight) && (get_motor_position_counter(LEFT_PORT_WHEEL) > -leftClicksRight))){
mav(RIGHT_PORT_WHEEL, CRUISING_SPEED_TURNS);
mav(LEFT_PORT_WHEEL, -CRUISING_SPEED_TURNS);
msleep(35);
}
}
else{
printf("Invalid input. LEFT and RIGHT globals accepted only\n");
}
}
void turnLeft(int speed, int time){
mav(LEFT_MOTOR_PORT, speed);
mav(RIGHT_MOTOR_PORT, (speed*-1));
msleep(time);
mav(RIGHT_MOTOR_PORT, 0);
mav(LEFT_MOTOR_PORT, 0);
}
inline int camera_move_y()
{
int speed = 200;
int back = -200;
blob_update();
if (current.green.y > (target.green.y - TOL))
{
printf("TOO CLOSE\n");
mav(lego.left.port , back);
mav(lego.right.port , back);
return 1;
}
if (current.green.y < (target.green.y + TOL))
{
printf("TOO FAR\n");
mav(lego.left.port , speed);
mav(lego.right.port , speed);
return 1;
}
if (current.green.y >= (target.green.y - TOL) && current.green.y <= (target.green.y + TOL))
{
printf("GOLDILOCKS\n");
ao();
return 0;
}
}
//right angle turn function
void rightAngleFwdMav(int direction, int debug ) {
int clickNbrTarget = RIGHT_ANGLE_CLICKS_LEFT;
int initial_position = get_motor_position_counter(RIGHT_MOTOR);
if (direction == RIGHT) {
clickNbrTarget = RIGHT_ANGLE_CLICKS_RIGHT;
initial_position = get_motor_position_counter(LEFT_MOTOR);
}
int current_position = initial_position;
if (direction == RIGHT) {
mav(LEFT_MOTOR,SPEED_FWD/2);
} else if (direction == LEFT) {
mav(RIGHT_MOTOR, SPEED_FWD/2) ;
}
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 fwd Init %d , curr %d, tgt %d\n", initial_position,current_position , clickNbrTarget );
}
}
mav(RIGHT_MOTOR, 0);
mav(LEFT_MOTOR, 0);
reset_motors();
}
void goStraight(int speed, int dist){ //negatives will go backwards, positives forwards. //dist = speed * time; time = dist/speed
mav(LEFT_MOTOR_PORT, speed);
mav(RIGHT_MOTOR_PORT, speed);
msleep(dist/speed); //check this forumula.
mav(LEFT_MOTOR_PORT, 0);
mav(RIGHT_MOTOR_PORT, 0);
}
void LinkSquareUp(int time, float speed) {
mav(RIGHT_PORT, speed*LINK_RIGHT_OFFSET);
mav(LEFT_PORT, speed*LINK_LEFT_OFFSET);
mav(3, 400); //Metal blocker lowered
msleep(time);
ao();
}
void LinkDriveSlanted(float rightSpeed, float IGUSTime, float time) {
mav(RIGHT_PORT,rightSpeed*LINK_RIGHT_OFFSET);
mav(LEFT_PORT, 1500*LINK_LEFT_OFFSET);
msleep(IGUSTime);
set_servo_position(ARM_PORT, ARM_OUT);
msleep(time - IGUSTime);
}
//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 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();
}
void turnCCW() {
mav(0,700);
mav(1,-700);
sleep(0.46);
ao();
}
void movecenterbackwards(int tspeed) {
mav(1, -tspeed);
mav(0, -tspeed+40);
void leftRed(int tspeed) {
mav(1, -tspeed);
mav(0, tspeed);
}
void reverseT(int speed, float seconds)
{
mav(motorL, -speed);
mav(motorR, -speed + drag);
sleep(seconds);
freeze(motorL);
freeze(motorR);
}
void move(int speed, int seconds) {
mav(leftWheel, speed);
mav(rightWheel, speed);
msleep(seconds * 1000);
mav(leftWheel, 0);
mav(rightWheel, 0);
}
void forwardT(int speed, float seconds)
{
mav(motorL, speed);
mav(motorR, speed - drag);
sleep(seconds);
freeze(motorL);
freeze(motorR);
}
void LinkDrive(float distance, float speed) {
mav(RIGHT_PORT, speed*LINK_RIGHT_OFFSET);
mav(LEFT_PORT, speed*LINK_LEFT_OFFSET);
clear_motor_position_counter(RIGHT_PORT);
while(get_motor_position_counter(RIGHT_PORT) < distance*CM_TO_TICKS) {
}
ao();
}
void blockMiddle() {
CardboardMid();
set_servo_position(2, 800);
msleep(200);
LinkDrive(3, 1600);
set_servo_position(2, 1500);
msleep(100);
set_servo_position(2, 800);
LinkDrive(3, 1600);
set_servo_position(2, 1500);
msleep(100);
set_servo_position(2, 800);
LinkDrive(3, 1600);
set_servo_position(2, 1500);
LinkDrive(10, 1600);
CardboardLow();
// LinkDrive(9, 1600);
/*
LinkDriveSlanted(870, 1200, 3500);
LinkSquareUp(1500, 2500);
LinkDriveSlanted(870, 2000, 3500);
LinkSquareUp(1500, 2500);
*/
mav(LEFT_PORT, 1000*LINK_LEFT_OFFSET);
mav(RIGHT_PORT, -1000*LINK_LEFT_OFFSET);
msleep(250);
LinkDrive(17, 1600);
// mav(LEFT_PORT, 1000*LINK_LEFT_OFFSET);
// msleep(30);
// mav(RIGHT_PORT, 0);
// mav(LEFT_PORT, 0);
// msleep(150);
LinkThrowIgus();
msleep(2000); //Waits for one second
/*
LinkSquareUp(1500, 1000);
LinkThrowArm();
LinkSquareUp(1500, 1500);
*/
/*
LinkDrive(20, 1500);
LinkTurnRightTime(1500, 500);
LinkSquareUp(3000, 1500);
LinkThrowArm();
*/
}
int main() {
setup();
printf("Hello World! \n I'm Martha and you're about to get rekt. \n");
printf("To the poms! \n");
move(1500,2);
system("clear");
printf("Munch munch munch... \n");
turn(1,1); // swing around to gather poms
sweeper(-1,10);
system("clear");
printf("To the poms! \n");
move(800,5);
system("clear");
printf("Munch munch munch... \n");
turn(2,-1); // swing around to gather poms
sweeper(-1,10);
system("clear");
printf("To the line! \n");
move(800,5);
system("clear");
lift();
move(100,1);
system("clear");
printf("Sorting... \n");
decide();
decide();
decide();
decide(); // eight times because eight poms
decide();
decide();
decide();
decide();
system("clear");
printf("Backing up... \n");
move(-500,1); // back up to avoid vertical projection of bins
msleep(1000);
mav(leftWheel,0);
mav(rightWheel,0);
return 0;
}
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();
}
void _arm_hold()
{
clear_motor_position_counter(1);
clear_motor_position_counter(3);
while (1)
{
mav(1, 100);
mav(3, 100);
msleep(1);
mav(1, -100);
mav(3, -100);
msleep(1);
}
}
void CW()
{
if (Side == right)
{
mav(motorR,-700); mav(motorL,700);
sleep(0.479999);
stop();
}
if (Side == left)
{
mav(motorR,700); mav(motorL,-700);
sleep(0.46);
stop();
}
}
void CCW180()
{
if (Side == right)
{
mav(motorR,700); mav(motorL,-700);
sleep(0.92);
stop();
}
if (Side == left)
{
mav(motorR,-700); mav(motorL,700);
sleep(0.959998);
stop();
}
}
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();
}
int main()
{
//startup();
set_servo_position(0,1230);
set_servo_position(3,1330);
mav(3,500);
mav(0,500);
sleep(2.8);
ao();
enable_servos();
set_servo_position(0,60);//90 degrees
set_servo_position(3,160);
sleep(sleep_time);
return 0;
}
void release() { // open claw until touch sensor #14 is activated
mav(3, -400);
while(digital(14) ==0) {
}
off(3);
clear_motor_position_counter(3);
}
void drivemm(float mmspeed, float seconds)//speed is mm/s
{
#ifdef CREATE
create_drive_straight(mmspeed);
sleep(seconds);
create_stop();
printf("%d mm/s * %d seconds = %d mm.\n", mmspeed, seconds, mmspeed*seconds);
#endif
#ifdef CBC
mav(0,mmspeed);
mav(1,mmspeed);
sleep(seconds);
off(0);
off(1);
#endif
}
void TurnCW()
{
#ifdef CREATE
while(get_create_total_angle(0.1) > -90)
{
create_spin_CW(500);
}
create_stop();
#endif
#ifdef CBC
mav(1,500);
mav(0,-500);
sleep(2.5900127);
off(0);
off(1);
#endif
}
