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 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 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 lego_drive_until_black_line(int direction, int speed, int line_size) {
int left_ticks_traveled = 0;
int right_ticks_traveled = 0;
float right_error;
float left_error;
float desired_value;
int left_threshold;
int right_threshold;
int right_sensor_value;
int left_sensor_value;
int left_multiplier;
int right_multiplier;
if (line_size == SMALL_LINE) {
left_threshold = LEFT_TOPHAT_SMALL_BLACK;
right_threshold = RIGHT_TOPHAT_SMALL_BLACK;
} else {
left_threshold = LEFT_TOPHAT_LARGE_BLACK;
right_threshold = RIGHT_TOPHAT_LARGE_BLACK;
}
clear_motor_position_counter(LEFT_MOTOR);
clear_motor_position_counter(RIGHT_MOTOR);
motor(LEFT_MOTOR, speed * direction);
motor(RIGHT_MOTOR, speed * direction);
while(TRUE) {
left_sensor_value = analog(LEFT_TOPHAT);
right_sensor_value = analog(RIGHT_TOPHAT);
if (left_sensor_value > left_threshold || right_sensor_value > right_threshold) {
break;
}
left_ticks_traveled = get_motor_position_counter(LEFT_MOTOR);
right_ticks_traveled = get_motor_position_counter(RIGHT_MOTOR);
desired_value = (left_ticks_traveled + right_ticks_traveled) / 2.0;
left_error = desired_value - left_ticks_traveled;
right_error = desired_value - right_ticks_traveled;
left_multiplier = (int) ((left_error * l_kP) + 0.5);
right_multiplier = (int) ((right_error * r_kP) + 0.5);
motor(LEFT_MOTOR, (speed * direction) + left_multiplier);
motor(RIGHT_MOTOR, (speed * direction) + right_multiplier);
}
freeze(LEFT_MOTOR);
freeze(RIGHT_MOTOR);
}
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
}
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
}
int main(int argc, char** argv) {
clear_motor_position_counter(1);
clear_motor_position_counter(2);
motor(1, 30);
motor(2, 30);
while (!side_button()) {
printf("pos: %i/%i\n", get_motor_position_counter(1), get_motor_position_counter(2));
msleep(10);
}
alloff();
return 0;
}
void lego_drive_centimeters(float centimeters, int direction, int speed) {
int left_ticks_traveled = 0;
int right_ticks_traveled = 0;
int ticks = (int) (centimeters * TICKS_PER_CENTIMETER);
float desired_value;
int left_multiplier;
int right_multiplier;
error current_error, previous_error, error_delta, errors_sum;
clear_motor_position_counter(LEFT_MOTOR);
clear_motor_position_counter(RIGHT_MOTOR);
previous_error.left = 0;
previous_error.right = 0;
errors_sum.left = 0;
errors_sum.right = 0;
motor(LEFT_MOTOR, speed * direction);
motor(RIGHT_MOTOR, speed * direction);
while(TRUE) {
if (abs(left_ticks_traveled) >= ticks || abs(right_ticks_traveled) >= ticks) {
break;
}
left_ticks_traveled = get_motor_position_counter(LEFT_MOTOR);
right_ticks_traveled = get_motor_position_counter(RIGHT_MOTOR);
desired_value = (left_ticks_traveled + right_ticks_traveled) / 2.0;
current_error.left = desired_value - left_ticks_traveled;
current_error.right = desired_value - right_ticks_traveled;
error_delta.left = current_error.left - previous_error.left;
error_delta.right = current_error.right - previous_error.right;
errors_sum.left = (errors_sum.left * 0.7) + current_error.left;
errors_sum.right = (errors_sum.right * 0.7) + current_error.right;
left_multiplier = (int) (((current_error.left * l_kP) + 0.5) + ((errors_sum.left * l_kI) + 0.5) + ((error_delta.left * l_kD) + 0.5));
right_multiplier = (int) (((current_error.right * r_kP) + 0.5) + ((errors_sum.right * r_kI) + 0.5) + ((error_delta.right * r_kD) + 0.5));
motor(LEFT_MOTOR, (speed * direction) + left_multiplier);
motor(RIGHT_MOTOR, (speed * direction) + right_multiplier);
previous_error = current_error;
}
freeze(LEFT_MOTOR);
freeze(RIGHT_MOTOR);
msleep(100);
}
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 release() { // open claw until touch sensor #14 is activated
mav(3, -400);
while(digital(14) ==0) {
}
off(3);
clear_motor_position_counter(3);
}
void servo_booster(int servo_port, int motor_port, int servo_position, int motor_position)
{
clear_motor_position_counter(motor_port);
motor(motor_port, 100);
set_servo_position(servo_port, servo_position);
while (get_motor_position_counter(motor_port) < motor_position);
off(motor_port);
}
/*
* Class: Motor
* Method: clear_motor_position_counter
* Signature: (I)I
*/
JNIEXPORT jint JNICALL Java_cbccore_low_Motor_clear_1motor_1position_1counter(JNIEnv* env, jobject obj, jint port)
{
#ifdef CBC
return clear_motor_position_counter(port);
#else
printf("Java_cbccore_low_Motor_clear_1motor_1position_1counter stub\n");
return -1;
#endif
}
void lego_drive_until_opening(int direction, int speed) {
int left_ticks_traveled = 0;
int right_ticks_traveled = 0;
float right_error;
float left_error;
float desired_value;
int left_multiplier;
int right_multiplier;
int left_sensor_value;
int right_sensor_value;
clear_motor_position_counter(LEFT_MOTOR);
clear_motor_position_counter(RIGHT_MOTOR);
motor(LEFT_MOTOR, speed * direction);
motor(RIGHT_MOTOR, speed * direction);
while(TRUE) {
left_sensor_value = analog_et(LEFT_ET);
right_sensor_value = analog_et(RIGHT_ET);
if (left_sensor_value < ET_NO_WALL && right_sensor_value < ET_NO_WALL) {
break;
}
left_ticks_traveled = get_motor_position_counter(LEFT_MOTOR);
right_ticks_traveled = get_motor_position_counter(RIGHT_MOTOR);
desired_value = (left_ticks_traveled + right_ticks_traveled) / 2.0;
left_error = desired_value - left_ticks_traveled;
right_error = desired_value - right_ticks_traveled;
left_multiplier = (int) ((left_error * l_kP) + 0.5);
right_multiplier = (int) ((right_error * r_kP) + 0.5);
motor(LEFT_MOTOR, (speed * direction) + left_multiplier);
motor(RIGHT_MOTOR, (speed * direction) + right_multiplier);
}
freeze(LEFT_MOTOR);
freeze(RIGHT_MOTOR);
}
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 turn_right()
{
motor(0,0);
motor(2,0);
clear_motor_position_counter(0);
mtp(0, 1000, 1050);
block_motor_done(0);
}
void turn_right()
{
motor(0,0);
motor(2,0);
clear_motor_position_counter(0);
mtp(0, 1000, 850); // this is what got changed from 1050 to 950 to 850
block_motor_done(0);
}
void turn_left()
{
motor(0,0);
motor(2,0);
clear_motor_position_counter(2);
mtp(2, 1500, 1300);
block_motor_done(2);
}
void lego_spin_degrees(int degrees, int direction, int speed) {
int left_ticks_traveled = 0;
int right_ticks_traveled = 0;
float right_error;
float left_error;
float desired_value;
int left_multiplier;
int right_multiplier;
int ticks = (int) ((float) (degrees) * TICKS_PER_DEGREE);
clear_motor_position_counter(LEFT_MOTOR);
clear_motor_position_counter(RIGHT_MOTOR);
motor(LEFT_MOTOR, speed * direction);
motor(RIGHT_MOTOR, -speed * direction);
while(TRUE) {
if (abs(left_ticks_traveled) >= ticks || abs(right_ticks_traveled) >= ticks) {
break;
}
left_ticks_traveled = abs(get_motor_position_counter(LEFT_MOTOR));
right_ticks_traveled = abs(get_motor_position_counter(RIGHT_MOTOR));
desired_value = (left_ticks_traveled + right_ticks_traveled) / 2.0;
left_error = desired_value - left_ticks_traveled;
right_error = desired_value - right_ticks_traveled;
left_multiplier = (int) ((left_error * spin_l_kP) + 0.5);
right_multiplier = (int) ((right_error * spin_r_kP) + 0.5);
motor(LEFT_MOTOR, (speed * direction) + left_multiplier);
motor(RIGHT_MOTOR, (-speed * direction) + right_multiplier);
}
freeze(LEFT_MOTOR);
freeze(RIGHT_MOTOR);
msleep(100);
}
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 lego_spin_until(int direction, int speed, int sensor, int (*comparator)(int, int), int threshold) {
int left_ticks_traveled = 0;
int right_ticks_traveled = 0;
float right_error;
float left_error;
float desired_value;
int left_multiplier;
int right_multiplier;
clear_motor_position_counter(LEFT_MOTOR);
clear_motor_position_counter(RIGHT_MOTOR);
motor(LEFT_MOTOR, speed * direction);
motor(RIGHT_MOTOR, -speed * direction);
while(TRUE) {
if (comparator(analog(sensor), threshold)) {
break;
}
display_printf(0, 0, "%4i", analog(sensor));
left_ticks_traveled = abs(get_motor_position_counter(LEFT_MOTOR));
right_ticks_traveled = abs(get_motor_position_counter(RIGHT_MOTOR));
desired_value = (left_ticks_traveled + right_ticks_traveled) / 2.0;
left_error = desired_value - left_ticks_traveled;
right_error = desired_value - right_ticks_traveled;
left_multiplier = (int) ((left_error * spin_l_kP) + 0.5);
right_multiplier = (int) ((right_error * spin_r_kP) + 0.5);
motor(LEFT_MOTOR, (speed * direction) + left_multiplier);
motor(RIGHT_MOTOR, (-speed * direction) + right_multiplier);
}
freeze(LEFT_MOTOR);
freeze(RIGHT_MOTOR);
}
void operate_winch(int position) {
raise_winch();
clear_motor_position_counter(WINCH_MOTOR);
if (position == WINCH_GROUND_POSITION) {
motor(WINCH_MOTOR, 100);
while (get_motor_position_counter(WINCH_MOTOR) < WINCH_GROUND_POSITION);
freeze(WINCH_MOTOR);
} else if (position == WINCH_MIDDLE_POSITION) {
motor(WINCH_MOTOR, 100);
while (get_motor_position_counter(WINCH_MOTOR) < WINCH_MIDDLE_POSITION);
freeze(WINCH_MOTOR);
}
adjust_motor(WINCH_MOTOR, position);
}
int main()
{
printf("Starting test of Blue and Green gripper.\n");
set_servo_position(BLUE_GRIPPER, BLUE_GRIPPER_START_POSITION);
set_servo_position(GREEN_GRIPPER, GREEN_GRIPPER_START_POSITION);
enable_servos();
press_A_to_continue();
set_servo_position(BLUE_GRIPPER, BLUE_GRIPPER_CLOSED_POSITION);
press_A_to_continue();
set_servo_position(GREEN_GRIPPER, GREEN_GRIPPER_CLOSED_POSITION);
press_A_to_continue();
printf("Starting test of spinner.\n");
clear_motor_position_counter(BLUE_SPINNER);
motor(BLUE_SPINNER, 100);
while (get_motor_position_counter(BLUE_SPINNER) < 225) ;
off(BLUE_SPINNER);
press_A_to_continue();
clear_motor_position_counter(BLUE_SPINNER);
motor(BLUE_SPINNER, 100);
while (get_motor_position_counter(BLUE_SPINNER) < 225) ;
off(BLUE_SPINNER);
press_A_to_continue();
clear_motor_position_counter(BLUE_SPINNER);
motor(BLUE_SPINNER, 100);
while (get_motor_position_counter(BLUE_SPINNER) < 225) ;
off(BLUE_SPINNER);
press_A_to_continue();
return 0;
}
void main ()
{
int loop_done = 0;
int task_A_done = 0;
int task_B_done = 0;
int angle0 = 0;
//get_create_total_ angle (.1);
//create_drive_direct (left speed, right speed)\
//move_to_position (motor #, motor power(-for backwards), end position)
//get_motor_done (motor#)
//clear_motor_position_counter (motor#)
//msleep (seconds)
//i'll probably use this often
create_connect ();
clear_motor_position_counter (0);
angle0 = get_create_total_angle (.1);
create_drive_direct (150,-150);
move_to_position (0, -900, -1550);
while(! loop_done)
{
printf("a0: %d, a: %d, d: %d\n", angle0, get_create_total_angle (.1), task_A_done);
if (angle0 - get_create_total_angle (.1) < 90)
{
create_drive_direct (150,-150);
}
else
{
task_A_done = 1;
}
if (get_motor_done (0))
{
}
else
{
task_B_done = 1;
}
if (task_A_done && task_B_done)
{
loop_done = 1;
}
msleep(0.1);
}
create_stop ();
create_disconnect ();
//put specific commands to control robot in here
// loop_done = 0;
// task_A_done = 0;
// task_B_done = 0;
// while(! loop_done)
// {
// if (/*task A is not done*/)
// {
// //continue doing task A
// }
// else
// {
// task_A_done = 1;
// }
// if (/*task B is not done*/)
// {
// //continue doing task B
// }
// else
// {
// task_B_done = 1;
// }
// if (task_A_done && task_B_done)
// {
// loop_done = 1;
// }
// }
}
void lego_spin_onto_black_line(int direction, int speed, int low_threshold, int high_threshold) {
int left_ticks_traveled = 0;
int right_ticks_traveled = 0;
float right_error;
float left_error;
float desired_value;
int left_multiplier;
int right_multiplier;
int i;
clear_motor_position_counter(LEFT_MOTOR);
clear_motor_position_counter(RIGHT_MOTOR);
motor(LEFT_MOTOR, speed * direction);
motor(RIGHT_MOTOR, -speed * direction);
for (i = 0; i < 3; i++) {
while(TRUE) {
if (analog_et(RIGHT_ET) >= low_threshold) {
break;
}
left_ticks_traveled = abs(get_motor_position_counter(LEFT_MOTOR));
right_ticks_traveled = abs(get_motor_position_counter(RIGHT_MOTOR));
desired_value = (left_ticks_traveled + right_ticks_traveled) / 2.0;
left_error = desired_value - left_ticks_traveled;
right_error = desired_value - right_ticks_traveled;
left_multiplier = (int) ((left_error * spin_l_kP) + 0.5);
right_multiplier = (int) ((right_error * spin_r_kP) + 0.5);
motor(LEFT_MOTOR, (speed * direction) + left_multiplier);
motor(RIGHT_MOTOR, (-speed * direction) + right_multiplier);
display_printf(0, 0, "L: %4i", ((speed * direction) + left_multiplier));
display_printf(0, 1, "R: %4i", ((speed * direction) + right_multiplier));
}
msleep(10);
while(TRUE) {
if (analog_et(RIGHT_ET) <= high_threshold) {
break;
}
left_ticks_traveled = abs(get_motor_position_counter(LEFT_MOTOR));
right_ticks_traveled = abs(get_motor_position_counter(RIGHT_MOTOR));
desired_value = (left_ticks_traveled + right_ticks_traveled) / 2.0;
left_error = desired_value - left_ticks_traveled;
right_error = desired_value - right_ticks_traveled;
left_multiplier = (int) ((left_error * spin_l_kP) + 0.5);
right_multiplier = (int) ((right_error * spin_r_kP) + 0.5);
motor(LEFT_MOTOR, (-speed * direction) + left_multiplier);
motor(RIGHT_MOTOR, (speed * direction) + right_multiplier);
display_printf(0, 0, "L: %4i", ((speed * direction) + left_multiplier));
display_printf(0, 1, "R: %4i", ((speed * direction) + right_multiplier));
}
}
freeze(LEFT_MOTOR);
freeze(RIGHT_MOTOR);
}
void main (int argc, char ** argv)
{
int loop_done = 0;
int task_A_done = 0;
int task_B_done = 0;
int angle = 0;
int dangle = 0;
int angle0 = 0;
int distance0 = 0;
int distance = 0 ;
int ddistance = 0;
// float lf_args[20];
// read inputs
// printf("Args: %d\n", argc);
// for (i = 2; i < argc; i++)
// {
// scanf("%lf%, argv[i]
// }
//get_create_total_ angle (.1);
//create_drive_direct (left speed, right speed)\
//move_to_position (motor #, motor power(-for backwards), end position)
//get_motor_done (motor#)
//clear_motor_position_counter (motor#)
//msleep (seconds)
//i'll probably use this often
create_connect ();
clear_motor_position_counter (0);
enable_servo (0);
set_servo_position (0 ,1300);
//set_servo_position (1800);
// ------------------------------------------------------------------------
//step:1 aim the arm to knock over the box
//raise arm
printf("-----Step 1-----\n");
move_to_position (0, -900, -1200);
while(! loop_done)
{
if (get_motor_done (0))
{
loop_done = 1;
}
msleep(100);
}
create_stop ();
loop_done = 0;
angle0 = get_create_total_angle (.05);
create_drive_direct (50,-50);
while(! loop_done)
{
printf("a0: %d, a: %d, d: %d\n", angle0, get_create_total_angle (.05), task_A_done);
if (angle0 - get_create_total_angle (.05) >= 20)
{
loop_done = 1;
create_stop ();
}
msleep(100);
}
loop_done = 0;
create_stop ();
task_A_done = 0;
task_B_done = 0;
angle0 = get_create_total_angle (.05);
create_drive_direct (-50,50);
while(! loop_done)
{
printf("a0: %d, a: %d, d: %d\n", angle0, get_create_total_angle (.05), task_A_done);
if (angle0 - get_create_total_angle (.05) <= -20)
{
loop_done = 1;
create_stop ();
}
msleep(100);
}
loop_done = 0;
create_stop ();
task_A_done = 0;
task_B_done = 0;
// ------------------------------------------------------------------------
//step:2 knock over box and turn
//lift arm
//turn 90
printf("-----Step 2-----\n");
move_to_position (0, -900, -3100);
msleep (900);
angle0 = get_create_total_angle (.05);
angle = angle0;
dangle = abs (angle0 - angle);
printf("a0: %d, a: %d, da: %d, d: %d\n", angle0, get_create_total_angle (.05), dangle , task_A_done);
create_drive_direct (-178,-648);
while(! loop_done)
{
if (dangle >= 125)
{
create_drive_direct (-120, -181);
}
if (dangle >= 155)
{
task_A_done = 1;
create_stop ();
}
if (get_motor_done (0))
{
task_B_done = 1;
}
if (task_A_done && task_B_done)
{
loop_done = 1;
}
msleep(100);
angle = get_create_total_angle (.05);
dangle = abs (angle0 - angle);
printf("a0: %d, a: %d, da: %d, d: %d\n", angle0, get_create_total_angle (.05), dangle , task_A_done);
}
loop_done = 0;
create_stop ();
task_A_done = 0;
task_B_done = 0;
// ------------------------------------------------------------------------
//step:4 aim claw to point of botguy
//turn 20
//lower arm
printf("-----Step 4-----\n");
angle0 = 0;
angle0 = get_create_total_angle (.05);
create_drive_direct (70, -70);
printf("a0: %d, a: %d\n", angle0, get_create_total_angle(0.05));
//while(angle0 - get_create_total_angle (.05) < 4)
if (0)
{
printf("a0: %d, a: %d\n", angle0, get_create_total_angle(0.05));
msleep(50);
}
move_to_position (0, 400, -900);
distance0 = get_create_distance (.05);
create_drive_direct (20, 20);
while (! loop_done)
{
if (get_create_distance (.05) - distance0 > 50)
{
loop_done = 1;
create_stop ();
}
}
set_servo_position (0 ,400);
loop_done = 0;
task_A_done = 0;
task_B_done = 0;
create_drive_straight (-20);
move_to_position (0, 600, -300);
while (! get_motor_done (0))
{
msleep (50);
}
create_drive_straight (-50);
msleep (2000);
//while (! loop_done)
if (0)
{
if (digital (10))
{
create_stop ();
loop_done = 1;
}
if (get_create_distance (.05) - distance0 < -100)
{
create_stop ();
loop_done = 1;
}
}
loop_done = 0;
// ------------------------------------------------------------------------
//step:5 grab botguy and lift him up (not complete)
printf("-----Step 5-----\n");
create_drive_direct (60, -60);
while(angle0 - get_create_total_angle (.05) < 3)
{
msleep(50);
}
create_stop ();
set_servo_position (0, 1510);
move_to_position (0, -1000, -3000);
msleep (5000);
// ------------------------------------------------------------------------
//step:6 move backwards untill the bumper hits the transport (not completed)
printf("-----Step 6-----\n");
create_drive_direct (130, 280);
distance0 = get_create_distance (.05);
while (! loop_done)
{
printf("d0: %d, d: %d, d: %d\n", distance0, get_create_distance (.05), loop_done);
if (get_create_distance (.05) - distance0 > 700)
{
loop_done = 1;
}
msleep(50);
}
loop_done = 0;
move_to_position (0, -900, -500);
create_drive_straight (-100);
msleep (1000);
set_servo_position (0, 200);
create_disconnect ();
printf("-----Step 6-B-----\n");
create_drive_direct (-400, -150);
distance0 = get_create_distance (.05);
while(! loop_done)
{
printf("d0: %d, d: %d, d: %d\n", distance0, get_create_distance (.05), loop_done);
if (get_create_distance (.05) - distance0 < -100)
{
loop_done = 1;
create_stop ();
}
msleep(50);
}
loop_done = 0;
// ------------------------------------------------------------------------
//step:7 drop botguy.
printf("-----Step 7-----\n");
move_to_position (0, -900, -300);
msleep (6000);
set_servo_position (0, 200);
create_disconnect ();
}
void MoveForwardMillimeters(int mmToGo, int Speed)
{
//Calculate the number of ticks to go
float RightTicksToGo = TICKS_PER_REVOLUTION_RIGHT*(mmToGo/MILLIMETERS_PER_REVOLUTION);
float LeftTicksToGo = TICKS_PER_REVOLUTION_LEFT*(mmToGo/MILLIMETERS_PER_REVOLUTION);
//Calculate mm per tick
float RightTicksPerMM = TICKS_PER_REVOLUTION_RIGHT/MILLIMETERS_PER_REVOLUTION;
float LeftTicksPerMM = TICKS_PER_REVOLUTION_LEFT/MILLIMETERS_PER_REVOLUTION;
//Set Motor Ticks position to zero
clear_motor_position_counter(LEFT_PORT_WHEEL);
clear_motor_position_counter(RIGHT_PORT_WHEEL);
//Set Motor Positions to a variable .. should be zero here because we just cleared them
int LeftMotorStartPosition = get_motor_position_counter(LEFT_PORT_WHEEL);
int RightMotorStartPosition = get_motor_position_counter(RIGHT_PORT_WHEEL);
//Calculate Motor Final Tick Position for MTP Command
int LeftMotorFinalPosition = LeftMotorStartPosition + LeftTicksToGo;
int RightMotorFinalPosition = RightMotorStartPosition + RightTicksToGo;
//Get time of start movement
//Not used yet.. for timeout functionality
double StartMovementTime = seconds();
//Loop until movment complete as measured by the left wheel
while(get_motor_position_counter(LEFT_PORT_WHEEL) < LeftMotorFinalPosition)
{
//Calculate Wheel Distance Moved each loop
int LeftDistanceMoved = (get_motor_position_counter(LEFT_PORT_WHEEL)-LeftMotorStartPosition)/LeftTicksPerMM;
int RightDistanceMoved = (get_motor_position_counter(RIGHT_PORT_WHEEL)-RightMotorStartPosition)/RightTicksPerMM;
//Calculate Wheel Split
//Positive Values Right Wheel is Ahead, Negative Values Left Wheel is ahead
int WheelSplit = RightDistanceMoved - LeftDistanceMoved;
//See if Wheel Split is exceeding the Correction Threshhold in either direction
if(WheelSplit < CORRECTION_THRESHOLD && WheelSplit > -CORRECTION_THRESHOLD)
{
//Wheels are going straight
motor(RIGHT_PORT_WHEEL, 100);
motor(LEFT_PORT_WHEEL, 100);
//move_to_position(RIGHT_PORT_WHEEL, Speed, RightMotorFinalPosition);
//move_to_position(LEFT_PORT_WHEEL, Speed, LeftMotorFinalPosition);
printf("STRAIGHT: left: %d right: %d split: %d\n", LeftDistanceMoved, RightDistanceMoved, WheelSplit);
}
else if(WheelSplit >= CORRECTION_THRESHOLD)
{
//Right wheel is ahead of left wheel
//correction reduction applied to right wheel
motor(RIGHT_PORT_WHEEL, 100*CORRECTION_REDUCTION);
motor(LEFT_PORT_WHEEL, 100);
//move_to_position(RIGHT_PORT_WHEEL, Speed * CORRECTION_REDUCTION, RightMotorFinalPosition);
//move_to_position(LEFT_PORT_WHEEL, Speed, LeftMotorFinalPosition);
printf("CORRECT RIGHT\n");
}
else if(WheelSplit <= -CORRECTION_THRESHOLD)
{
//Left wheel is ahead of right wheel
//correction reduction applied to left wheel
motor(RIGHT_PORT_WHEEL, 100);
motor(LEFT_PORT_WHEEL, 100*CORRECTION_REDUCTION);
//move_to_position(RIGHT_PORT_WHEEL, Speed, RightMotorFinalPosition);
//move_to_position(LEFT_PORT_WHEEL, Speed * CORRECTION_REDUCTION, LeftMotorFinalPosition);
printf("CORRECT LEFT\n");
}
msleep(150);
}
//Turn All Motors Off
ao();
}
int main()
{
/* FIRST CALIBRATION: based on poms position adjust starting position
*/
printf("test 3.11, calibration value: \nclaw down : %d, \nright ang forward right: %d, \nright ang forward left : %d, \nback right angle %d ...\n", DOWN_SERVO, RIGHT_ANGLE_CLICKS_MRP_RIGHT, RIGHT_ANGLE_CLICKS_MRP_LEFT, RIGHT_ANGLE_CLICKS_BACK_MRP);
double start_time = seconds();
enable_servos();
clear_motor_position_counter(RIGHT_MOTOR);
clear_motor_position_counter(LEFT_MOTOR);
wait_for_light(1);
shut_down_in(118);
clawUp();
//**************************************
//* PART 1 : get first set of poms to lower storage area
//*
//**************************************
moveBackward(1, NO_DEBUG);
moveForwardHighSpeed(23, DEBUG);
clawDown();
//moveForward(8,NO_DEBUG);
//we have the Poms
rightAngleBwd(RIGHT, NO_DEBUG);
//bump against the upper storage area
moveBackwardHighSpeed(15, NO_DEBUG);
moveForwardHighSpeed(17, NO_DEBUG);
rightAngleBwd(RIGHT, NO_DEBUG);
//bump against upper PVC side
moveBackwardHighSpeed(18, NO_DEBUG);
//move towards dropping poms
moveForward(1.5, NO_DEBUG);
msleep(300);
rightAngleFwd(LEFT, NO_DEBUG);
msleep(300);
clawUp();
//uses IR sensor to stop the forward movement
moveForwardTilBlackLine(15, NO_DEBUG);
printf("====> elapsed time end of part 1: %f\n", (seconds() - start_time));
//**************************************
//* END OF PART 1 : get first set of poms to lower storage area
//*
//**************************************
//**************************************
//* PART 2 : get the blue cube to the corner
//*
//**************************************
moveBackwardHighSpeed(25, NO_DEBUG);
//recalibrate against top PVC next to botguy
rightAngleBwd(LEFT, NO_DEBUG);
moveBackwardHighSpeed(11, NO_DEBUG);
// SECOND CALIBRATION: based on blackline/blue position
moveForward(7.5, NO_DEBUG);
rightAngleFwd(RIGHT, NO_DEBUG);
//get the cube now
//====>>>>> may have to be adjust the day of competition based on position of 2nd
//====>>>>> set of poms
moveForwardHighSpeed(15, NO_DEBUG);
//====>>>>>
clawAlmostDownCube();
twentyTwoAngleFwd(LEFT, NO_DEBUG);
clawUp();
//====>>>>> calibration to get to the corner 22.5 fwd, 22.5 more forward, aiming at bottom left hand corner
moveForwardHighSpeed(2.5, NO_DEBUG);
twentyTwoAngleFwd(LEFT, NO_DEBUG);
//clawAlmostDownCube();
moveForwardHighSpeed(30, NO_DEBUG);
//====>>>>>
//moveForwardHighSpeed(2, NO_DEBUG);
//====>>>>>
moveBackwardHighSpeed(9, NO_DEBUG);
fortyFiveAngleFwd(LEFT, NO_DEBUG);
printf("====> elapsed time end of part 2: %f\n", (seconds() - start_time));
//**************************************
//* END OF PART 2 : get the blue cube in the corner
//*
//**************************************
//**************************************
//* PART 3 : prepare to get second set of poms
//*
//**************************************
//recalibrate against side PVC
rightAngleBwd(RIGHT, NO_DEBUG);
moveBackwardHighSpeed(25, NO_DEBUG);
moveForward(5, NO_DEBUG);
//calibrating with top PVC
rightAngleBwd(LEFT,NO_DEBUG);
moveBackwardHighSpeed(23,NO_DEBUG);
// THIRD CALIBRATION: based on second set of Poms
//below is the number of inches from the side PVC
//
moveForwardHighSpeed(10, NO_DEBUG);
rightAngleBwd(RIGHT, NO_DEBUG);
//recalibrate with side pvc
moveBackwardHighSpeed(5,NO_DEBUG);
printf("====> elapsed time end of part 3: %f\n", (seconds() - start_time));
//**************************************
//* END OF PART 3 : prepare to get second set of poms
//*
//**************************************
//**************************************
//* PART 4 : bring second set of poms home
//*
//**************************************
//grabs the poms, bring down claw and go home
moveForwardHighSpeed(12.5,NO_DEBUG);
clawDown();
moveForwardHighSpeed(59.5, NO_DEBUG);
//here is the code to recalibrate one more time... but do we have enough time?
/*//recalibrate against top PVC
rightAngleBwd(LEFT,NO_DEBUG);
moveBackwardHighSpeed(10, NO_DEBUG);
//move towards dropping poms
moveForwardHighSpeed(10, NO_DEBUG);
//msleep(500);
printf("==> moving righ angle bwd\n");
rightAngleFwd(LEFT, NO_DEBUG);
*/
//recalibrate against top PVC
//moveForwardHighSpeed(41,NO_DEBUG);
//last calibration before dropping second set of poms
rightAngleBwd(LEFT,NO_DEBUG);
moveBackwardHighSpeed(14, NO_DEBUG);
//move towards dropping poms
moveForwardSlow(1.5, NO_DEBUG);
msleep(300);
rightAngleFwd(LEFT, NO_DEBUG);
msleep(300);
clawUp();
moveForwardTilBlackLine(15, DEBUG);
//**************************************
//* END OF PART 4 : bring second set of poms home
//*
//**************************************
printf("====> elapsed time: %f\n", (seconds() - start_time));
ao();
disable_servos();
return 0;
}
void CombinedMotorWidget::clearPosition()
{
clear_motor_position_counter(ui->motors->currentIndex());
}
