// if deg > 0, turn right, else turn left.
void turn(int deg)
{
if (deg > 0) {
motor(LEFT, 0);
} else {
motor(0, RIGHT);
}
Wait(TN * abs(deg));
stop();
}
void turn_right()
{
motor(0,0);
motor(2,0);
clear_motor_position_counter(0);
mtp(0, 1000, 1050);
block_motor_done(0);
}
int init()// move the grabber motors to the start position.
{
motor(1,-100); // move motor 1 at -100%
motor(3,-100); // move motor 3 at -100%
msleep(500); // stop moving the motors after .5 seconds
set_create_distance(0); // initializes the distance accumulation to start at 0
set_create_total_angle (0); // initializes the angle accumulation to start at 0
ao(); // stop all motors
return 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 junction(string *junction_string){
/*
This code stops the robot at an intersection to make sure you can chose for it to go straight left or right.
In case the direction is already chosen it just goes to the already chosen direction
*/
if (SensorValue[S1] < 38 && SensorValue[S2] < 55){//was 50
setMultipleMotors(50, motorA, motorB);//Drive the cart forward a little for 40 miliseconds. This way it ends up more straight on the line after turning
wait1Msec(50);
stopTask(music);//stops the music
clearSounds();//clears the sound buffer
setMultipleMotors(0, motorA, motorB);//stop the robot
wait(0.02);
while (1){
if(*junction_string == "LEFT"){//if the input is "LEFT" turn left until you read the black line color and then continue your normal duty
motor(motorA) = 0;
motor(motorB) = 40;
*junction_string = "";
while(1){
if (SensorValue[S1] < 40){//color sensor check
setMultipleMotors(0, motorA, motorB);
break;
}
}
break;
}
else if(*junction_string == "RIGHT"){//the same only then for turning right
motor(motorA) = 40;
motor(motorB) = 0;
*junction_string = "";
while(1){
if (SensorValue[S2] < 65){//light sensor check
setMultipleMotors(0, motorA, motorB);
break;
}
}
break;
}
//if up just stop the loop to exit
else if(*junction_string == "UP"){
setMultipleMotors(50, motorA, motorB);
wait(0.2);
break;
}
else if (*junction_string == ""){//When no input stop the robot
setMultipleMotors(0, motorA, motorB);
check_bleutooth(junction_string);
}
}
}
}
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 findBall(int color)
{
camera_update();
display_clear();
int count = 1;
int objNum = get_object_count(color);
while(objNum == 0)
{
camera_update();
objNum = get_object_count(color);
}
printf("%d time finding a ball", count);
point2 objCen = get_object_center(color, 0);
int errorX = 0, errorY = 0;
errorX = OFFSET_X - objCen.x;
errorY = OFFSET_Y - objCen.y;
while(!(a_button_clicked()))
{
if(BALL_NUM_BOX >= 2)
break;
if(errorX > -4 && errorX < 4 && errorY > -4 && errorY < 4)
break;
int turnLM = -1 * errorX * P_X + errorY * P_Y;
int turnRM = errorX * P_X + errorY * P_Y;
if(turnLM > -1 * MINS && turnLM < 0)
turnLM = -1 * MINS;
if(turnLM > 0 && turnLM < MINS)
turnLM = MINS;
if(turnRM > -1 * MINS && turnRM < 0)
turnRM = -1 * MINS;
if(turnRM > 0 && turnRM < MINS)
turnRM = MINS;
motor(LM, turnLM);
motor(RM, turnRM);
camera_update();
int objArea = get_object_area(color, 0);
while(objArea < 200)
{
ao();
camera_update();
objArea = get_object_area(color, 0);
}
count++;
display_clear();
printf("%d time finding a ball ", count);
printf("%d NUM", BALL_NUM_BOX);
objCen = get_object_center(color, 0);
errorX = OFFSET_X - objCen.x;
errorY = OFFSET_Y - objCen.y;
}
ao();
catchBalls(color);
}
void claw_down(){
motor(Motor_Up,Motor_down_speed);
while(!digital(Sensor_Down)){}
freeze(Motor_Up);
//wait so it doesn't f**k up
msleep(500);
//up for straffes seil
motor(Motor_Up,Motor_up_speed);
while(!digital(Sensor_Down)){}
freeze(Motor_Up);
}
void moveLine(){
double timeStart = seconds();
double timeEnd = seconds();
while((timeEnd - timeStart) < 1){
int error = analog10(LSS) - LSS_OFFSET;
int turn = error * MOVE_LINE_Kp;
turn = turn / 10000;
motor(LM, MOVE_LINE_Tp - turn);
motor(RM, MOVE_LINE_Tp + turn);
timeEnd = seconds();
}
}
void moveLine(){
double timeStart = seconds();
double timeEnd = seconds();
while((timeEnd - timeStart) < 2){
int error = analog10(LSS) - LSS_OFFSET;
int turn = error * Kp;
turn = turn / 10000;
motor(LM, 80 + turn);
motor(RM, 80 - turn);
timeEnd = seconds();
}
}
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 kill() {
printf("killing in %i.", wait * 1000);
msleep(wait * 1000);
printf("killing program.\n");
disable_servos();
create_drive_straight(0);
motor(0, 0);
motor(1, 0);
motor(2, 0);
motor(3, 0);
exit(1);
}
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 bringback2cube(){
//claw up
motor(Motor_Up,Motor_up_speed);
//zurück 1s
drive(1000,-Drivespeed,-Drivespeed);
//turn right 90
drive(970,Turnspeed,-Turnspeed);
//vor to calibrate
drive(1000,Drivespeed,Drivespeed);
//back
drive(500,-Drivespeed,-Drivespeed);
//turn so little bit b4 cubes
drive(1130,Turnspeed,-Turnspeed);
//vor
drive(900,Drivespeed,Drivespeed);
//wait for claw up
claw_up();
//camera fix
camera_update();
//hide your cubes
cube_is_near();
//back
drive(1000,-Drivespeed,-Drivespeed);
//turn left more than 90 idk
drive(1330,-Turnspeed,Turnspeed);
//start to down motor
motor(Motor_Up,Motor_down_speed);
//vor to calibrate
drive(1000,Drivespeed,Drivespeed);
//langsamer weil wackeldackel
drive(2000,Drivespeed_middle+20,Drivespeed_middle+20);
//back
drive(400,-Drivespeed,-Drivespeed);
//turn more than 90 lulz
drive(980,-Turnspeed,Turnspeed);
//light left and shit
drive(5000,Drivespeed_middle*2,(Drivespeed_middle*2)-7);
//light back and shit
drive(1000,-Drivespeed_middle*2,(-Drivespeed_middle*2)+7);
//wait for claw down
while(!digital(Sensor_Down)){
if(seconds()>start+113){
claw_open();
disable_servos();
}
}
freeze(Motor_Up);
msleep(2000);
claw_open();
disable_servos();
}
void initializeRobot()
{
int nDelay = 200;
bFloatDuringInactiveMotorPWM = false;
motor(Left_Side) = 0;
motor(Right_Side) = 0;
for (int i = 0; i < 20; i++) {
avgdata += SensorValue[Gyro];
wait1Msec(25);}
}
void find_line_position_again(){
printf("find line again \n");
stop();
msleep(500);
motor(RIGHT_MOTOR,-70);
while(analog10(RIGHT_SENSOR)<right_blk-150){}
stop();
msleep(500);
motor(RIGHT_MOTOR,-70);
while(analog10(RIGHT_SENSOR)>right_blk-150){}
}
void Camera::followContour(int j) {
int i = getXContour(j);
if(i != -1) {
int vel1 = -VELOCIDADE_MAX + 4*VELOCIDADE_MAX*((double)i/MAXXCOOR);
int vel2 = 3*VELOCIDADE_MAX - 4*VELOCIDADE_MAX*((double)i/MAXXCOOR);
if(vel1 > VELOCIDADE_MAX) vel1 = VELOCIDADE_MAX;
else if (vel1 < -VELOCIDADE_MAX) vel1 = -VELOCIDADE_MAX;
if(vel2 > VELOCIDADE_MAX) vel2 = VELOCIDADE_MAX;
else if(vel2 < -VELOCIDADE_MAX) vel2 = -VELOCIDADE_MAX;
motor(vel1, vel2);
}
else motor(50,50);
}
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 wait_for_kill(void) {
while (!side_button()) {}
alloff();
motor(0, 0);
motor(1, 0);
motor(2, 0);
motor(3, 0);
alloff();
disable_servos();
create_drive_direct(0, 0);
create_stop();
create_disconnect();
exit(0);
}
int main()
{
double timeStart = seconds();
double timeEnd = seconds();
while((timeEnd - timeStart) < 2) {
int error = analog10(LSS) - LSS_OFFSET;
int turn = error * Kp;
turn = turn / 10000;
motor(LM, Tp - turn);
motor(RM, Tp + turn);
timeEnd = seconds();
}
return 0;
}
void junction() {
/*
This code stops the robot at an intersection to make sure you can chose for it to go straight left or right.
In case the direction is already chosen it just goes to the already chosen direction
*/
if (SensorValue[S1] < (mincolor + 10) && SensorValue[S2] < (minlight + 10)) {//the minimul light value + 10 for the correct moment to stop at a line
setMultipleMotors(50, motorA, motorB); //Drive the cart forward a little for 50 miliseconds. This way it ends up more straight on the line after turning
wait1Msec(50);
stopTask(music); //stops the music
clearSounds(); //clears the sound buffer
setMultipleMotors(0, motorA, motorB); //stop the robot
wait(1);
while (1) {
nxtDisplayTextLine(0, "Counter: %d %c", counter, new_matrix[counter]);
if (new_matrix[counter] == 'L') {//if the input is "LEFT" turn left until you read the black line color and then continue your normal duty
motor(motorA) = 0;
motor(motorB) = 40;
while (1) {
if (SensorValue[S1] < 30) {//color sensor check
wait1Msec(5);
setMultipleMotors(0, motorA, motorB);
counter++;
break;
}
}
break;
} else if (new_matrix[counter] == 'R') {//the same only then for turning right
motor(motorA) = 40;
motor(motorB) = 0;
while (1) {
if (SensorValue[S2] < 50) {//light sensor check
wait1Msec(5);
setMultipleMotors(0, motorA, motorB);
counter++;
break;
}
}
break;
} //if up just stop the loop to exit
else if (new_matrix[counter] == 'U') {
setMultipleMotors(50, motorA, motorB);
wait(0.2);
counter++;
break;
}
}
}
}
void left(float degrees, float radius) {
int turnlspeed;
long turnl=((2*radius-ks)*CONVERSION*PI)*(degrees/360.);
long turnr=((2*radius+ks)*CONVERSION*PI)*(degrees/360.);
if(turnr == 0l)
return;
turnlspeed = round((float)turnl / (float)turnr*SPD);
msleep(30l);
if(turnr > 0l)
motor(MOT_RIGHT, SPD);
else
motor(MOT_RIGHT, -SPD);
if(turnlspeed < 0) turnlspeed = -turnlspeed;
if(turnl > 0l)
motor(MOT_LEFT, turnlspeed);
else
motor(MOT_LEFT, -turnlspeed);
turnr += gmpc(MOT_RIGHT);
turnl += gmpc(MOT_LEFT);
if(turnl - gmpc(MOT_LEFT) > 0l){
if(turnr - gmpc(MOT_RIGHT) > 0l){
while((turnl > gmpc(MOT_LEFT) && turnlspeed != 0) || turnr > gmpc(MOT_RIGHT)) {
if(turnl < gmpc(MOT_LEFT) - 10l)
off(MOT_LEFT);
if(turnr < gmpc(MOT_RIGHT) - 10l)
off(MOT_RIGHT);
}
}else{
while((turnl > gmpc(MOT_LEFT) && turnlspeed != 0) || turnr < gmpc(MOT_RIGHT)) {
if(turnl < gmpc(MOT_LEFT) - 10l) off(MOT_LEFT);
if(turnr > gmpc(MOT_RIGHT) + 10l) off(MOT_RIGHT);
}
}
}else{
if(turnr - gmpc(MOT_RIGHT) > 0l) {
while((turnl < gmpc(MOT_LEFT) && turnlspeed != 0) || turnr > gmpc(MOT_RIGHT)) {
if(turnl > gmpc(MOT_LEFT) + 10l) off(MOT_LEFT);
if(turnr < gmpc(MOT_RIGHT) - 10l) off(MOT_RIGHT);
}
} else {
while((turnl < gmpc(MOT_LEFT) && turnlspeed != 0) || turnr < gmpc(MOT_RIGHT)) {
if(turnl > gmpc(MOT_LEFT) + 10l) off(MOT_LEFT);
if(turnr > gmpc(MOT_RIGHT) + 10l) off(MOT_RIGHT);
}
}
}
drive_off();
msleep(30l);
}
void initializeRobot()
{
servo[Claw] = 110; //Sets Claw to closed position
bFloatDuringInactiveMotorPWM = false;
motor(B_L) = 0; //Sets left drive motors to 0
motor(B_R) = 0; //Sets right drive motors to 0
for (int i = 0; i < 20; i++)
{
avgdata += SensorValue[Gyro]; //Takes 20 measurements of the gyro
wait1Msec(25); //Pauses for 250 miliseconds
}
}
void adjust_onto_line(int direction) {
if (analog(L_TOPHAT) > 750 && analog(R_TOPHAT) > 750) {
return;
} else if (analog(L_TOPHAT) > 750 && analog(R_TOPHAT) < 750) {
motor(RIGHT_MOTOR, 30 * direction);
while (analog(R_TOPHAT) < 750) {}
off(RIGHT_MOTOR);
} else if (analog(L_TOPHAT) < 750 && analog(R_TOPHAT) > 750) {
motor(LEFT_MOTOR, 30 * direction);
while (analog(L_TOPHAT) < 750) {}
off(LEFT_MOTOR);
} else if (analog(L_TOPHAT) < 750 && analog(R_TOPHAT) < 750) {
printf("ROBOT IS NOT ON BLACK LINE!\n");
}
}
task main()
{
while(true)
{
motor(lefttop) = vexRT[ch3];
motor(leftbot) = -vexRT[ch3];
motor(rightbot) = vexRT[ch2];
motor(righttop) = -vexRT[ch2];
if(vexRT[Btn6U]==1)
{
motor[armr1] = motor[armr2] = 127;
motor[arml1] = motor[arml2] = -127;
}
else
{
motor[armr1] = motor[armr2] = 0;
motor[arml1] = motor[arml2] = 0;
}
if(vexRT[Btn6D]==1)
{
motor[armr1] = motor[armr2] = -127;
motor[arml1] = motor[arml2] = 127;
}
else
{
motor[armr1] = motor[armr2] = 0;
motor[arml1] = motor[arml2] = 0;
}
if(vexRT[Btn5U]==1)
{
motor[score] = -127;
}
else
{
motor[score] = 0;
}
if(vexRT[Btn5D]==1)
{
motor[score] = 127;
}
else
{
motor[score] = 0;
}
}
}
void forward(int n)
{
Wait(0);
motor(LEFT, RIGHT);
Wait(50);
stop();
}
