//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(); }
void Right_AngleMAV(int Direction, double timeLimit){ long rightClicksRight=0.6*TICKS_PER_REVOLUTION_RIGHT; long leftClicksRight=0.6*TICKS_PER_REVOLUTION_LEFT; clear_motor_position_counter(LEFT_PORT_WHEEL); clear_motor_position_counter(RIGHT_PORT_WHEEL); double startTURN90Time = seconds(); while((seconds() - startTURN90Time <= timeLimit) && (get_motor_position_counter(LEFT_PORT_WHEEL) < leftClicksRight) && (get_motor_position_counter(LEFT_PORT_WHEEL) > -leftClicksRight)){ if(Direction==RIGHT){ mrp(RIGHT_PORT_WHEEL, (int)CRUISING_SPEED_TURNS_RIGHT, -rightClicksRight); mrp(LEFT_PORT_WHEEL, (int)CRUISING_SPEED_TURNS_LEFT, leftClicksRight); bmd(RIGHT_PORT_WHEEL); bmd(LEFT_PORT_WHEEL); } else if(Direction==LEFT){ mrp(RIGHT_PORT_WHEEL, (int)CRUISING_SPEED_TURNS_RIGHT, rightClicksRight); mrp(LEFT_PORT_WHEEL, (int)CRUISING_SPEED_TURNS_LEFT, -leftClicksRight); bmd(RIGHT_PORT_WHEEL); bmd(LEFT_PORT_WHEEL); } else{ printf("Invalid input. LEFT and RIGHT globals accepted only\n"); } }//Close While }
//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(); }
int goForwardPastLaptop(int moveSpeed) { mrp(leftMotor,moveSpeed,pastLaptop); mrp(rightMotor,moveSpeed,pastLaptop); msleep(5000); return 0; }
int leftTurnCP(int turnSpeed) { mrp(leftMotor,turnSpeed,-TurnCP); mrp(rightMotor,turnSpeed,TurnCP); msleep(3000); return 0; }
int rightTurnCP(int turnSpeed) { // move_to_position wouldn't work here. mrp(leftMotor,turnSpeed,TurnCP); //move_relative_position mrp(rightMotor,turnSpeed,-TurnCP); msleep(3000); return 0; }
//uses mrp (move to relative position) and convert from inches //to motor units. This is pretty accurate (a lot more than motor at speed) void moveForward(int distanceInInches) { //printf("starting to move for %d\n",distanceInInches); //convert inches to clicks long clicks = 156.25l * distanceInInches; mrp(RIGHT_MOTOR, SPEED_FWD, clicks); mrp(LEFT_MOTOR, SPEED_FWD, clicks); bmd(RIGHT_MOTOR); bmd(LEFT_MOTOR); //printf("done moving %d...", distanceInInches); }
void dump_kelp_de() { servo kelp_arm = build_servo(GRAB_PORT, 440, 1650, 15, 5); servo kelp_grab = build_servo(KELP_PORT, 300, 1810, 15, 5); wait_servo(kelp_arm, KELP_SCORE + 200); wait_servo(kelp_grab, GRAB_SCORE); mrp(DUMP_PORT, DUMP_SPEED, DUMP_UP); bmd(DUMP_PORT); mrp(DUMP_PORT, DUMP_SPEED / 2, -DUMP_UP); bmd(DUMP_PORT); off(DUMP_PORT); }
void fortyFiveAngleFwd(int direction) { if (direction == RIGHT) { //printf("test turning right"); mrp(LEFT_MOTOR,SPEED_FWD,FV_ANGLE_CLICKS); bmd(LEFT_MOTOR); } else if (direction == LEFT) { //printf ("test turning 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"); } }
void rightAngleBwd(int direction) { if (direction == RIGHT) { //printf("test turning right"); mrp(LEFT_MOTOR,SPEED_BWD,RIGHT_ANGLE_CLICKS_BACK); bmd(LEFT_MOTOR); } else if (direction == LEFT) { //printf ("test turning left"); mrp(RIGHT_MOTOR, SPEED_BWD, RIGHT_ANGLE_CLICKS_BACK) ; bmd(RIGHT_MOTOR); } else { printf("ooopppsss I did not recognize your turn... so I ignored it"); } }
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(); }
void findLine(){ int LSS_CHECK = 0; int RSS_CHECK = 0; while(LSS_CHECK == 0 || RSS_CHECK == 0){ if (analog10(LSS) > LSS_OFFSET) LSS_CHECK = 1; if (analog10(RSS) > RSS_OFFSET) RSS_CHECK = 1; motor(LM, LM_SP_STR1); motor(RM, RM_SP_STR1); } mrp(LM, LM_SP_STR2, 250); mrp(RM, RM_SP_STR2, 250); bmd(LM); bmd(RM); ao(); }
// move: void move(int order){ if(order == 1){ mrp(LM, LM_SP_STR2, BALL_1_DIS); mrp(RM, RM_SP_STR2, BALL_1_DIS); bmd(LM); bmd(RM); ao(); } if(order == 2){ findLine(); turnLeftLine(); moveLine(); } if(order == 3){ } }
/* * Class: Motor * Method: mrp * Signature: (III)I */ JNIEXPORT jint JNICALL Java_cbccore_low_Motor_mrp(JNIEnv* env, jobject obj, jint port, jint speed, jint delta) { #ifdef CBC return mrp(port, speed, delta); #else printf("Java_cbccore_low_Motor_mrp stub\n"); return -1; #endif }
void moveToBalls(){ camera_update(); camera_update(); camera_update(); camera_update(); int objNum = get_object_count(GREEN); while(objNum == 0){ motor(LM, LM_SP_STR1); motor(RM, RM_SP_STR1); camera_update(); objNum = get_object_count(GREEN); } mrp(LM, -1 * LM_SP_STR2, 120); mrp(RM, -1 * RM_SP_STR2, 120); bmd(LM); bmd(RM); ao(); //msleep(250); }// move forward; stop when sees a target ball
void FORTE_MotorPOS::executeEvent(int pa_nEIID){ //TODO MotorPOS add error checks and messages QO() = QI(); switch (pa_nEIID){ case scm_nEventINITID: if(true == QI()){ if(PORT() <= 3){ libcbc_init(); //TODO: only call once //initialises the CBC Library :) m_nPort = PORT(); QO() = true; /* Turned on and No Error */ } else{ QO() = false; } } sendOutputEvent(scm_nEventINITOID); break; case scm_nEventREQID: if(true == QI()){ if(true == RELPOS()) mrp(m_nPort, 1000, POS()); else mtp(m_nPort, 1000, POS()); } sendOutputEvent(scm_nEventCNFID); break; case scm_nEventRSPID: //TODO respond to IND break; case scm_nEventSTOPID: if(true == QI()){ motor(m_nPort, 0); if(true == FREEZE()){ freeze(m_nPort); } else{ off(m_nPort); } } sendOutputEvent(scm_nEventSTOPOID); break; } }
int rightTurnOP(int turnSpeed) { mrp(leftMotor,turnSpeed,TurnOP); msleep(3000); return 0; }
int main() { struct cbcRobot robot = {0,3,150,55,1100}; //Define the robot. 0 and 3 are the motor ports, //150 mm is the wheel distance //55 is wheel diameter, 1100 is ticks per rotation light_it_up(4); //Wait for the light to turn on set_servo_position(arm_servo, top_pos); //Set the arm servo to the top position shut_down_in(118.0); //Shut down in 118 seconds //msleep(5000); //Wait 5 seconds float initial_time=seconds(); //Create a variable that records the initial time cbc_align(400); //Align with both of the top hats on the black line ao(); cbc_align_white(300); lower_arm(600); //Lower claw and open arm open_claw(600); cbc_straight(140, 800); //Straight out to get away from pvc wait_for_cbc(); cbc_arc_left(160, 135, 100); //Get past pvc to lower arm and open claw wait_for_cbc(); cbc_straight(180, 800); //Go straight into tribbles wait_for_cbc(); close_claw(1000); //Close claw on tribbles cbc_straight(280, 800); wait_for_cbc(); raise_arm(1200); //Raise the arm msleep(800); cbc_arc_right(180, 34, 80); //Arc right to get into position to approach the lattice wall wait_for_cbc(); cbc_touch(700, 5000); //Bump into lattice wall msleep(300); cbc_straight(-100, 600); //Back up 10 cm from wall wait_for_cbc(); open_claw(800); //Open claw to release tribbles and turn 94 degrees right cbc_spin(-94, 500); wait_for_cbc(); cbc_straight(120, 700); //Go forward into the turnstile and lower arm to mid position arm_mid(2000); msleep(1200); cbc_spin(20, 500); //Turn 20 degrees left to hit the turnstile out of the way wait_for_cbc(); msleep(200); cbc_spin(-16, 700); //Turn back 16 degrees to the right and close the claw close_claw(800); wait_for_cbc(); cbc_straight(-280, 800); //Go backwards to get into position to grab the tribbles wait_for_cbc(); lower_arm(1000); //Lower the arm and open the claw msleep(800); open_claw(800); msleep(1000); cbc_spin(8,700); //Turn 8 degrees to get ready to grab the tribbles wait_for_cbc(); cbc_arc_left(1500, 6, 100); //Go nearly straight into the tribbles wait_for_cbc(); close_claw(200); //Go forward and grab the tribbles cbc_straight(200, 600); wait_for_cbc(); close_claw(600); msleep(800); cbc_straight(-60, 600); //Back up to clear the turnstile raise_arm(1000); wait_for_cbc(); cbc_spin(4,700); //Turn slightly left wait_for_cbc(); follow_tape_left(600); //Run into the turnstile for horizonal alignment cbc_straight(-100, 400); //Back up before crossing over to the other side wait_for_cbc(); cbc_spin(90, 500); //Turn 90 degrees to the left wait_for_cbc(); cbc_straight(-80, 600); //Back up and align with the black tape wait_for_cbc(); cbc_align(400); ao(); cbc_straight(-50, 300); //Back up and align with the black tape wait_for_cbc(); cbc_align(400); ao(); cbc_straight(540,800); //Go forward to the other side wait_for_cbc(); open_claw(1000); //Open the claw to release tribbles and arc right approximately 100 degrees mrp(robot.leftWheel,780,2340); mrp(robot.rightWheel,400,1200); bmd(robot.leftWheel); bmd(robot.rightWheel); msleep(200L); lower_arm(1400);//Go forward and lower the arm msleep(500L); cbc_straight(370, 800); wait_for_cbc(); close_claw(1000); //Go forward and close the claw cbc_straight(300,800); wait_for_cbc(); raise_arm(1500); //Raise the arm msleep(800); cbc_spin(-73,500); //Turn 73 degrees right wait_for_cbc(); open_claw(1000); //Run into the lattice wall and open the claw to release the tribbles cbc_touch(500, 5000); cbc_straight(-80, 600); //Back up 8 cm from wall wait_for_cbc(); cbc_spin(-92, 500); //Turn 92 degrees right and close the claw close_claw(1000); wait_for_cbc(); cbc_straight(-80, 600); //Go backwards to get into position to grab the tribbles wait_for_cbc(); lower_arm(1500); //Lower the arm and open the claw msleep(800); open_claw(800); msleep(1000); cbc_spin(4,700); //Turn a bit in order to put the robot in an optimal tribble gathering position wait_for_cbc(); cbc_arc_left(1500, 6, 100); //Go nearly straight into the tribbles wait_for_cbc(); close_claw(200); //Go forward and grab tribbles cbc_straight(180, 600); wait_for_cbc(); close_claw(600); msleep(800); cbc_straight(-80, 400); //Back up to clear the turnstile wait_for_cbc(); raise_arm(1200); //Raise the arm msleep(500); cbc_straight(220, 700); //Go forward into the turnstile wait_for_cbc(); cbc_spin(80, 500); //Turn 80 degrees to the left wait_for_cbc(); cbc_straight(-80, 600); //Back up and align with the black tape wait_for_cbc(); cbc_align(400); ao(); cbc_straight(-50, 300); //Back up and align with the black tape wait_for_cbc(); cbc_align(400); ao(); //cbc_straight(1080, 800); //Old go forward function //wait_for_cbc(); mrp(robot.rightWheel,825,7700); //Go forward and correct the non-straightness of the motors mrp(robot.leftWheel,800,7700); wait_for_cbc(); cbc_touch(500, 5000); //Run into the wall msleep(300); cbc_straight(-125, 600); //Back up 12.5 cm from wall wait_for_cbc(); cbc_spin(90, 500); //Turn 90 degrees to the left wait_for_cbc(); while(analog10(et_right)<850) //While the tape is not seen by the right top hat { mav(robot.rightWheel,615); //Go forward mav(robot.leftWheel,600); msleep(25L); } off(robot.rightWheel); //Turn the motors off off(robot.leftWheel); cbc_straight(240, 700); //Go forward to get under the injection chute wait_for_cbc(); msleep(8000); //Wait for the injection chute tribbles to be loaded up cbc_straight(250, 700); //Go forward into the wall wait_for_cbc(); cbc_straight(-40, 600); //Back up 4 cm from pvc wait_for_cbc(); arm_mid(1000); //Lower the arm to the mid position and open the claw msleep(800); open_claw(800); msleep(800); cbc_straight(-40, 600); //Back up 4 cm from pvc wait_for_cbc(); close_claw(1000); //Turn around 180 degrees and close the claw cbc_spin(180,600); wait_for_cbc(); cbc_straight(-80, 480); //Back up to wall wait_for_cbc(); lower_arm(1400); //Lower the arm msleep(800L); mrp(dump_mot, 200, 500); //Dump the tribble into the MPA bmd(dump_mot); msleep(1000); mrp(dump_mot, 200, -500); //Dump the tribble into the MPA bmd(dump_mot); mrp(dump_mot, 200, 500); //Dump the tribble into the MPA bmd(dump_mot); mrp(dump_mot, 200, -500); //Dump the tribble into the MPA bmd(dump_mot); float time = seconds() - initial_time; //Create a variable that represents the time taken for the robot to complete its program cbc_display_clear(); //Clear the CBC display and then print the time the robot took to complete its program printf("Time: %f",time); return 0; }