示例#1
0
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);
  
}
示例#2
0
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;
	}
}
示例#3
0
 ~Sling() {
   arm.move_to( iPair(0,0) );
   mav(motor_shoulder, -200);
   mav(motor_arm, -200);
   sleep(0.2);
   ao();
 }
示例#4
0
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);
}
示例#6
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;
	}
}
示例#7
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);
}
示例#9
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();
}
示例#10
0
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);
}
示例#11
0
//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();
}
示例#12
0
//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);
}
示例#16
0
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);
}
示例#18
0
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();
}
示例#19
0
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();	
    */
}
示例#20
0
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;
}
示例#21
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();
}
示例#22
0
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();
	}
}
示例#25
0
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();
}
示例#26
0
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;
}
示例#28
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);
}
示例#29
0
	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
	
	}
示例#30
0
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
	}