//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 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
}
void turnRight(){
clear_motor_position_counter(LM);
clear_motor_position_counter(RM);
mtp(LM, 1500,HALF_CIRCLE);
mtp(RM, -1500, -1 * HALF_CIRCLE);
bmd(LM);
bmd(RM);
}
void turnBack(){
clear_motor_position_counter(LM);
clear_motor_position_counter(RM);
mtp(LM, 1500, ONE_CIRCLE);
mtp(RM, -1500, -1 * ONE_CIRCLE);
bmd(LM);
bmd(RM);
}
// move:
void moveStraight(int distance)
{
clear_motor_position_counter(c_ileftMotor);
clear_motor_position_counter(c_irightMotor);
mtp(c_ileftMotor, c_iforwardSpeed, distance);
mtp(c_irightMotor, c_iforwardSpeed, distance);
bmd(c_ileftMotor);
bmd(c_irightMotor);
}
void collectBall(){
motor(FM, FM_COLLECT);
clear_motor_position_counter(LM);
clear_motor_position_counter(RM);
mtp(LM, LM_SP_STR2, 800);
mtp(RM, RM_SP_STR2, 800);
bmd(LM);
bmd(RM);
motor(FM, 0);
// move and collect rest balls
}
//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 collectBall(){
motor(FM, FM_COLLECT);
clear_motor_position_counter(LM);
clear_motor_position_counter(RM);
mtp(LM, 1500, 1000);
mtp(RM, 1500, 1000);
bmd(LM);
bmd(RM);
motor(FM, 0);
msleep(2500);
// move and collect rest balls
}
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 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 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 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 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: bmd
* Signature: (I)V
*/
JNIEXPORT void JNICALL Java_cbccore_low_Motor_bmd(JNIEnv* env, jobject obj, jint motor)
{
#ifdef CBC
return bmd(motor);
#else
printf("Java_cbccore_low_Motor_bmd stub\n");
#endif
}
void moveFor(){
int frontDis = analog_et(FSS);
int backDis = analog_et(BSS);
int disDiff = frontDis - backDis;
int turnLM = MOVE_FOR_TP - disDiff * KDp;
int turnRM = MOVE_FOR_TP + disDiff * KDp;
while(frontDis > 295){
motor(LM, turnLM);
motor(RM, turnRM);
frontDis = analog_et(FSS);
backDis = analog_et(BSS);
printf("%d\t%d\n", frontDis, backDis);
disDiff = frontDis - backDis;
turnLM = MOVE_BACK_TP - disDiff * KDp;
turnRM = MOVE_BACK_TP + disDiff * KDp;
}
clear_motor_position_counter(LM);
clear_motor_position_counter(RM);
mtp(LM, 800, 500);
mtp(RM, 800, 500);
bmd(LM);
bmd(RM);
clear_motor_position_counter(LM);
clear_motor_position_counter(RM);
mtp(LM, 800, 260);
mtp(RM, -800, -260);
bmd(LM);
bmd(RM);
while(analog(RSS) < RSS_OFFSET){
motor(RM, 82);
motor(LM, 90);
}
while(analog(RSS) > RSS_OFFSET){
motor(RM, 82);
motor(LM, 90);
}
while(analog(RSS) < RSS_OFFSET){
motor(RM, 82);
motor(LM, 90);
}
}
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
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;
}
