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 }