//right angle turn function
void rightAngleBwdMav(int direction, int debug ) {
mav(RIGHT_MOTOR, 0);
mav(LEFT_MOTOR, 0);
msleep(100);
int clickNbrTarget = RIGHT_ANGLE_CLICKS_BACK;
int initial_position = get_motor_position_counter(RIGHT_MOTOR);
if (direction == RIGHT) {
clickNbrTarget = RIGHT_ANGLE_CLICKS_BACK;
initial_position = get_motor_position_counter(LEFT_MOTOR);
}
int current_position = initial_position;
if (direction == RIGHT) {
mav(LEFT_MOTOR,SPEED_BWD);
} else if (direction == LEFT) {
mav(RIGHT_MOTOR, SPEED_BWD) ;
}
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 bwd mav Init %d , curr %d, tgt %d\n", initial_position,current_position , clickNbrTarget );
}
}
mav(RIGHT_MOTOR, 0);
mav(LEFT_MOTOR, 0);
reset_motors();
}
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");
}
}
//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();
}
//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 lower_bay(void) {
motor(kMotorPortBayLeft, 10);
motor(kMotorPortBayRight, 10);
while (get_motor_position_counter(kMotorPortBayLeft) < 230 || get_motor_position_counter(kMotorPortBayRight) < 230) {
printf("pos: %i/%i\n", get_motor_position_counter(kMotorPortBayLeft), get_motor_position_counter(kMotorPortBayRight));
}
motor(kMotorPortBayLeft, 0);
motor(kMotorPortBayLeft, 0);
}
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);
}
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 raise_bay(void) {
while (get_motor_position_counter(kMotorPortBayLeft) > 30 || get_motor_position_counter(kMotorPortBayRight) > 30) {
motor(kMotorPortBayLeft, -50);
motor(kMotorPortBayRight, -50);
msleep(200);
//motor(kMotorPortBayLeft, 0);
//motor(kMotorPortBayRight, 0);
//msleep(200);
printf("pos: %i/%i\n", get_motor_position_counter(kMotorPortBayLeft), get_motor_position_counter(kMotorPortBayRight));
}
motor(kMotorPortBayLeft, 0);
motor(kMotorPortBayLeft, 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 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);
}
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 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: get_motor_position_counter
* Signature: (I)I
*/
JNIEXPORT jint JNICALL Java_cbccore_low_Motor_get_1motor_1position_1counter(JNIEnv* env, jobject obj, jint port)
{
#ifdef CBC
get_motor_position_counter(port);
#else
printf("Java_cbccore_low_Motor_enable_1servos 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);
}
double SensorsWidget::rawValue(const int &i) const
{
double val = 0;
if(i < 8) val = analog10(i);
else if(i < 12) val = get_motor_position_counter(i - 8);
else if(i == 12) val = accel_x();
else if(i == 13) val = accel_y();
else if(i == 14) val = accel_z();
return val;
}
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 CombinedMotorWidget::update()
{
#ifdef A_KOVAN
publish();
#endif
int port = ui->motors->currentIndex();
ui->position->setText(QString::number(get_motor_position_counter(port)));
ui->positionStop->setEnabled(get_motor_done(port) ? false : true);
ui->go->setEnabled(get_motor_done(port) ? true : false);
}
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 backward(float distance){
if(distance < 0.) {
forward(-distance);
return;
}
// Calculate the # of ticks the robot must move for each wheel
long ticks = CMtoBEMF(distance);
long totalLeftTicks = get_motor_position_counter(MOT_LEFT) - ticks;
long totalRightTicks = get_motor_position_counter(MOT_RIGHT) - ticks;
// Start motors
motor(MOT_LEFT, -SPDl);
motor(MOT_RIGHT, -SPDr);
// Keep moving until both motors reach their desired # of ticks
while(get_motor_position_counter(MOT_LEFT) > totalLeftTicks
&& get_motor_position_counter(MOT_RIGHT) > totalRightTicks) {
if (get_motor_position_counter(MOT_LEFT) <= totalLeftTicks)
off(MOT_LEFT);
if (get_motor_position_counter(MOT_RIGHT) <= totalRightTicks)
off(MOT_RIGHT);
}
off(MOT_LEFT);
off(MOT_RIGHT);
}
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 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);
}
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;
}
double PidTunerWidget::getFeedbackValue()
{
#ifndef NOT_A_KOVAN
publish();
#endif
int position = get_motor_position_counter(ui->motor->currentIndex());
double vel = (1.0-LPF_ALPHA) * m_vel_1
+ LPF_ALPHA * (1000 / UPDATE_MS) * (position - m_position_1) / MOTOR_SCALING;
m_position_1 = position;
m_vel_1 = vel;
return vel;
}
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 Move_X_Millimeters(int Direction, float distMillimeters, double timeLimit){
float rightClicksStraight = TICKS_PER_REVOLUTION_RIGHT*(distMillimeters/MILLIMETERS_PER_REVOLUTION);
float leftClicksStraight = TICKS_PER_REVOLUTION_LEFT*(distMillimeters/MILLIMETERS_PER_REVOLUTION);
clear_motor_position_counter(LEFT_PORT_WHEEL);
clear_motor_position_counter(RIGHT_PORT_WHEEL);
if(Direction==FORWARD){
//printf("CRUISING_SPEED_FWD_LEFT is: %f:\n", CRUISING_SPEED_FWD_LEFT);
double ticksDistance = TICKS_PER_REVOLUTION * (distMillimeters/MILLIMETERS_PER_REVOLUTION);
clear_motor_position_counter(RIGHT_PORT_WHEEL);
clear_motor_position_counter(LEFT_PORT_WHEEL);
int motor_left_position_start = get_motor_position_counter(LEFT_PORT_WHEEL);
int motor_right_position_start = get_motor_position_counter(RIGHT_PORT_WHEEL);
double startFWDTime = seconds();
while((get_motor_position_counter(LEFT_PORT_WHEEL) < (motor_left_position_start + ticksDistance))
&& (seconds() - startFWDTime <= timeLimit)){
int mvt_counter_left = get_motor_position_counter(LEFT_PORT_WHEEL)-motor_left_position_start;
int mvt_counter_right = get_motor_position_counter(RIGHT_PORT_WHEEL)-motor_right_position_start;
int wheel_difference = mvt_counter_right - mvt_counter_left;
if(wheel_difference < CORRECTION_THRESHOLD && wheel_difference > -CORRECTION_THRESHOLD){//If both are within the same range, wheels are going straight
mav(LEFT_PORT_WHEEL, CRUISING_SPEED_FWD);
mav(RIGHT_PORT_WHEEL, CRUISING_SPEED_FWD);
msleep(5);
printf("STRAIGHT: left: %d right: %d diff: %d\n", mvt_counter_left, mvt_counter_right, wheel_difference);
}
else if(mvt_counter_right > mvt_counter_left){//right wheel is ahead of left wheel
//mav(LEFT_PORT_WHEEL, 750);
mav(RIGHT_PORT_WHEEL, CRUISING_SPEED_FWD * CORRECTION_REDUCTION); //correction speeed is 80% of fwd speed
printf("RIGHT diff: %d\n", wheel_difference);
msleep(35);
}
else if(mvt_counter_left > mvt_counter_right){//left wheel is ahead of right wheel
mav(LEFT_PORT_WHEEL, CRUISING_SPEED_FWD * CORRECTION_REDUCTION); //correction speeed is 80% of fwd speed
//mav(RIGHT_PORT_WHEEL, 750);
printf("LEFT diff: %d\n", wheel_difference);
msleep(35);
}
msleep(15);
}
ao();
}
if(Direction==BACK){
double ticksDistance = -TICKS_PER_REVOLUTION * (distMillimeters/MILLIMETERS_PER_REVOLUTION);
clear_motor_position_counter(RIGHT_PORT_WHEEL);
clear_motor_position_counter(LEFT_PORT_WHEEL);
int motor_left_position_start = get_motor_position_counter(LEFT_PORT_WHEEL);
int motor_right_position_start = get_motor_position_counter(RIGHT_PORT_WHEEL);
double startBACKTime = seconds();
while((get_motor_position_counter(LEFT_PORT_WHEEL) > (motor_left_position_start + ticksDistance))
&& (seconds() - startBACKTime <= timeLimit)){
printf("BACK\n");
int mvt_counter_left = get_motor_position_counter(LEFT_PORT_WHEEL)-motor_left_position_start;
int mvt_counter_right = get_motor_position_counter(RIGHT_PORT_WHEEL)-motor_right_position_start;
int wheel_difference = mvt_counter_right - mvt_counter_left;
if(wheel_difference < CORRECTION_THRESHOLD && wheel_difference > -CORRECTION_THRESHOLD){//If both are within the same range, wheels are going straight
mav(LEFT_PORT_WHEEL, CRUISING_SPEED_BWD);
mav(RIGHT_PORT_WHEEL, CRUISING_SPEED_BWD);
msleep(5);
printf("left: %d right: %d diff: %d\n", mvt_counter_left, mvt_counter_right, wheel_difference);
}
else if(mvt_counter_right > mvt_counter_left){ //right wheel is ahead of left
//mav(LEFT_PORT_WHEEL, 750);
mav(LEFT_PORT_WHEEL, CRUISING_SPEED_BWD * CORRECTION_REDUCTION); //correction speed is 80% of BWD Speed
printf("correcting right diff: %d\n", wheel_difference);
msleep(35);
}
else if(mvt_counter_left > mvt_counter_right){// left wheel is ahead of right
mav(RIGHT_PORT_WHEEL, CRUISING_SPEED_BWD * CORRECTION_REDUCTION); //correction speed is 80% of BWD Speed
//mav(RIGHT_PORT_WHEEL, 750);
printf("correcting left diff: %d\n", wheel_difference);
msleep(35);
}
msleep(15);
}
ao();
}
}
void findBall(){
display_clear();
int temptLMSpeed = LM_MINS; // changeable minimum speed for left motor
int temptRMSpeed = RM_MINS; // changeable minimum speed for right morot
int lastLMSpeed, lastRMSpeed; // motor speed
int lastLMPos, lastRMPos; // motor positions
int errorX = 0, errorY = 0; // difference of current ball coordinate and target coordinate
int objArea; // area of the largest object in sight
point2 objCen; // object center coordinate
camera_update();
int objNum = get_object_count(GREEN);
while(objNum == 0){
camera_update();
objNum = get_object_count(GREEN);
}
// find a ball in sight
objCen = get_object_center(GREEN, 0);
errorX = OFFSET_X - objCen.x;
errorY = OFFSET_Y - objCen.y;
while(!(a_button_clicked())){
camera_update();
objArea = get_object_area(GREEN, 0);
while(objArea < 200){
ao();
camera_update();
objArea = get_object_area(GREEN, 0);
}
// make sure really a ball is in sight
objCen = get_object_center(GREEN, 0);
errorX = OFFSET_X - objCen.x;
errorY = OFFSET_Y - objCen.y;
// find differences between coordinates
if(errorX > -3 && errorX < 2 && errorY > -3 && errorY < 2)
break;
int turnLM = -1 * errorX * P_X + errorY * P_Y;
int turnRM = errorX * P_X + errorY * P_Y;
// turn for LM, RM
if(turnLM == lastLMSpeed && lastLMPos == get_motor_position_counter(LM))
temptLMSpeed = temptLMSpeed + 1;
if(turnLM == lastLMSpeed && lastLMPos != get_motor_position_counter(LM))
temptLMSpeed = temptLMSpeed - 1;
if(turnLM == -1 * lastLMSpeed && lastLMPos != get_motor_position_counter(LM))
temptLMSpeed = temptLMSpeed - 2;
if(turnRM == lastRMSpeed && lastRMPos == get_motor_position_counter(RM))
temptRMSpeed = temptRMSpeed + 1;
if(turnRM == lastRMSpeed && lastRMPos != get_motor_position_counter(RM))
temptRMSpeed = temptRMSpeed - 1;
if(turnRM == -1 * lastRMSpeed && lastRMPos != get_motor_position_counter(RM))
temptRMSpeed = temptRMSpeed - 2;
lastLMSpeed = turnLM;
lastLMPos = get_motor_position_counter(LM);
lastRMSpeed = turnRM;
lastRMPos = get_motor_position_counter(RM);
// check if speed is too high or low, thus make modification to minimum speed
if(turnLM > -1 * temptLMSpeed && turnLM < 0)
turnLM = -1 * temptLMSpeed;
if(turnLM > 0 && turnLM < temptLMSpeed)
turnLM = temptLMSpeed;
if(turnRM > -1 * temptRMSpeed && turnRM < 0)
turnRM = -1 * temptRMSpeed;
if(turnRM > 0 && turnRM < temptRMSpeed)
turnRM = temptRMSpeed;
// set minimum speed: avoid unmoving
motor(LM, turnLM);
motor(RM, turnRM);
// move
}
ao();
catchBall(GREEN);
// stop and catch
}
int main(int argc, char** argv) {
enable_servo(kServoPortSorter);
set_servo_position(kServoPortSorter, kServoPositionSorterCenter);
clear_motor_position_counter(kMotorPortBayLeft);
clear_motor_position_counter(kMotorPortBayRight);
create_connect();
camera_open(LOW_RES);
scanf("%s", NULL);
printf("waiting for light\n");
while (analog(0) > 200) {}
shut_down_in(700000);
motor(kMotorPortBayLeft, 10);
motor(kMotorPortBayRight, 10);
while (get_motor_position_counter(kMotorPortBayLeft) < 230 || get_motor_position_counter(kMotorPortBayRight) < 230) {
printf("pos: %i/%i\n", get_motor_position_counter(kMotorPortBayLeft), get_motor_position_counter(kMotorPortBayRight));
}
motor(kMotorPortBayLeft, 0);
motor(kMotorPortBayLeft, 0);
// wait_for_side_button();
msleep(6000);
create_drive_straight(200);
msleep(1850);
create_spin_CCW(200);
msleep(750);
create_drive_straight(-200);
msleep(1500);
create_spin_CW(200);
msleep(1750);
create_drive_straight(200);
while (!get_create_lbump() && !get_create_rbump()) {}
create_drive_straight(-150);
msleep(900);
create_spin_CCW(200);
msleep(950);
create_drive_straight(150);
while (!get_create_lbump() && !get_create_rbump()) {}
create_stop();
msleep(10000);
thread all_off = thread_create(wait_for_kill);
thread_start(all_off);
raise_bay();
create_spin_CW(100);
msleep(500);
create_drive_straight(-100);
msleep(1000);
create_stop();
thread jiggle_c = thread_create(jiggle_create);
thread_start(jiggle_c);
thread sort_b = thread_create(sort_balls);
thread_start(sort_b);
msleep(33000);
while (true) {}
/*thread_destroy(jiggle_c);
create_stop();
lower_bay();
create_drive_straight(100);
while (!get_create_lbump() && !get_create_rbump()) {}
create_stop();
msleep(10000);
raise_bay();
create_spin_CW(100);
msleep(500);
create_drive_straight(-100);
msleep(1000);
create_stop();
thread_create(jiggle_create);
thread_start(jiggle_c);
thread_create(sort_balls);
while (true) {}
camera_close();*/
return 0;
}
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();
}
//moves forward with light sensor
void moveForwardRoutine(double distanceInInches, int checkIRSensor, int speed, int debug) {
//checkLightSensor do not check light sensor: see #define values
// do check light sensor and stop when it is over black or void
//convert inches to clicks
int speed_adjust_up = (int) (speed / ADJUST_SPEED);
int speed_adjust_down = (int) (speed * ADJUST_SPEED);
printf("speed down %d, speed up: %d\n", speed_adjust_down, speed_adjust_up);
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);
float differential = 0 ;
//we will keep moving until both motors have covered their distance or
//the light sensor is tripped
while (current_position_left <= (initial_position_left + clicks) ||
current_position_right <= (initial_position_right + clicks) ) {
//let's get out if the sensor is on
if (checkIRSensor == CHECK_IR_SENSOR && analog(IR_SENSOR) > IR_SENSOR_THRESHOLD) {
if (debug == DEBUG) {
printf("exiting because of light sensor");
}
break;
}
//first let's see if one motor is going ahead of the other
differential = (current_position_left - initial_position_left)
* MOTOR_CORRECTION_FACTOR_LEFT ;
differential = differential - (current_position_right - initial_position_right);
// if (debug == DEBUG) {
// printf("sensor value: %d lightsensor %d \n", analog(IR_SENSOR),checkIRSensor);
// }
if (differential > -25 && differential < 25 ) {
//counter are around the same
mav(RIGHT_MOTOR, speed);
mav(LEFT_MOTOR, speed);
} else if (differential < 0 ) {
//right has moved ahead, let's slow down right until left catches up
mav(RIGHT_MOTOR,speed_adjust_down);
mav(LEFT_MOTOR, speed_adjust_up);
if (debug == DEBUG) {
printf("adjusting LEFT L: %d R: %d, diff %f\n", (current_position_left - initial_position_left), (current_position_right - initial_position_right), differential);
}
} else {
//left has moved ahead, let's slow down left until right catches up
mav(RIGHT_MOTOR, speed_adjust_up);
mav(LEFT_MOTOR, speed_adjust_down);
if (debug == DEBUG) {
printf("adjusting RIGHT L: %d R: %d diff %f\n", (current_position_left - initial_position_left), (current_position_right - initial_position_right), differential);
}
}
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();
}
