BOTSBY

#include <kipr/wombat.h>
//ran tests, got true 90 degrees as 1101 in turn function, not good for the turn after the cup line lineup tho

//some values are kinda innaccurate and it lowkey turns
//weird sometimes but it seems to have more potential than the other

/*
|___________________________________________|
|    1    |    2     |   3      |    4      | 
| Blue P  | Yellow CA| YellowCB | Orange P  |
|===========================================|
|    5    |    6     |   7      |    8      | 
| Red C   | Green P  | Red P    | Green C   |
|___________________________________________|
*/


//ports
const int lightPort = 0;
int lm = 3;  //left motor
int rm = 0;  // right motor

int left_ir = 2; //left ir CHECK SENSORS
int right_ir = 1; //right ir CHECK SENSORS
int back_left_ir = 4;
int back_right_ir = 3;

int base = 1; // base of arm, directly on chassis
int arm = 2; //second servo, pivots arm
int claw = 3; //controls claw opening and closing
int sweep = 0;

int sweep_out = 300; //1740
int sweep_in = 1850; //

//movement
int speed = 200; //motor speed
int fiddle_speed = 50; //used to adjust for stopline
int turn_speed = 100; //used for turn function


//servo values
int realBase = 1600;
int realArm = 1900;
int realClaw = 1996;
int realSweep = 1850;

const int bStart = 1600;
const int aStart = 1900;

const int bCenter = 1250;
const int aCenter = 1300;

const int bDrop = 1990; //was 1850    MATTHEW
const int aDrop = 250;

const int cOpen = 1996;
const int cClose = 150;

const int bPomA = 866;
const int aPomA = 1836; //values for grabbing the close sets of poms

const int bCube = 840; //values for grabbing small cubes
const int aCube = 1748;
const int cCube = 600;

const int bBigCube = 300;
const int aBigCube = 900;
const int cBigCube = 800;

const int bPomB = 150;
const int aPomB = 900;

//sensor values
int tape = 2900; //black tape on ir sensors     ADJUST FOR LIGHT; WAS 3300, SERIOUS RUNS/ON THE 14TH


//functions
void drive(int direction, int speed, int distance); // 2000 for distance, 22.4 cm  INPUT IN CM
void turn(int direction, int turn_speed, int distance);
void stopTuah(int speed, int fiddle_speed);
void stopTuahBack (int speed, int fiddle_speed);
void sweepWater();
void lineCalibration();
void rangeRead();
int set_servos (int basePos, int armPos, int clawPos, double delay);	//YOU CAN ONLY USE EVEN NUMBERS NOW
void grabPomA();
void grabCube();
void grabBigCube();
void grabPomB();
void start_servos();
void centerArm();
void grabBigCube2();

//------------MAIN FUNCTION---------------------------------
int main()
{
 	start_servos();
	//wait_for_light(lightPort);
    shut_down_in(199);
    
    //Beginning section
    stopTuah(800, 200);			//lines up on starting box tape
    drive(1,900, 30);
    stopTuah(300,15);			//lines up on cup box tape
    drive(-1, 300, 1.5); 
    turn(-1, 400, 1095);   		//90 degree turn left changed from 1080 to 1095
	
    //First Section
    drive(1, 800, 19);
    sweepWater();			//first water
    drive(1, 800, 12); 
    ao();
    grabPomB();				//collecting cube and poms
    drive(1, 800, 1); 
    ao();
    grabCube();
    
    //Second Section
    drive(1, 800, 10);
    sweepWater();			//second water CHANGE THE VALUE OF HOW FAR THE SERVO GOES, IT WAS REPLACED
    drive(1, 800, 15);
    ao();
    grabBigCube2();			//first entree (Cube2, has a backwards inch bc the cube was on the black line many times)
    grabPomA();
    drive(1, 900, 15);
    printf("halfway on board");
    sweepWater();	//third water
    
    //Third section
    drive(1, 900, 17);
    stopTuahBack(400, 15);	//lines up far side of middle line
    ao();
    grabBigCube();
    drive(-1, 900, 1);
    ao();
    grabPomA();
    
    //Fourth section
    drive(1, 900, 11);
    sweepWater(); //fourth water
    drive(1, 900, 12);
    ao();
    grabPomB();
    drive(1, 800, 1); 
    ao();                                     //collecting green cubes and orange poms
    grabCube();
    
    //Driving to end box and resetting
    turn(-1, 600, 350);
    drive(1, 900, 20);
    turn(-1, 600, 640);
    drive(1, 1000, 33);
    set_servo_position(sweep, realSweep);
    ao();
    start_servos();
    return 0;
}


//--------------functions------------------
void drive(int direction, int speed, int distance)
{
    cmpc(lm);
    cmpc(rm);
    distance = distance * 100.015873;  //WAS 101.1206896
    if (direction > 0) //forward if 1
    { 
        while(( ( abs(gmpc(rm)) + abs(gmpc(lm)) ) / 2 ) < distance) // takes the clear motors clear value and moves until its reached the distance
        {
            mav(lm,speed);
            mav(rm,speed);
        }
    }
    if (direction < 0) //back if 1
    { 
        while(( ( abs(gmpc(rm)) + abs(gmpc(lm)) ) / 2 ) < distance) // takes the clear motors clear value and moves until its reached the distance
        {
            mav(lm,-speed);
            mav(rm,-speed);
        }
    }
    ao();
    cmpc(lm);
    cmpc(rm);
}
void turn (int direction, int turn_speed, int distance)   // positive = CCW, negative = CW +=left - = right
{
    cmpc(lm);
    cmpc(rm);
    if (direction < 0) //turn left
    {
    	while(( (abs(gmpc(rm)) + abs(gmpc(lm)) ) / 2) < distance)
        {
       	    mav(lm,abs(turn_speed));
            mav(rm,-abs(turn_speed));
        } 

    }
    if (direction > 0) // turn right
    {
        while(( (abs(gmpc(rm)) + abs(gmpc(lm)) ) / 2) < distance)
        {
            mav(lm,-abs(turn_speed));
            mav(rm,abs(turn_speed));
        } 

    }
    cmpc(lm);
    cmpc(rm);
}

void lineCalibration(){
    stopTuah (100, 20); //back up to black line
    cmpc(lm);
    cmpc(rm);
    drive(1, 50, 1);
    msleep(2000);
    stopTuah(100, 20); //move up to black line, measure
    printf("left: %d\n", gmpc(lm) );
    printf("right: %d\n", gmpc(rm) );
}

void rangeRead(){
	int i = 0;
    int temp = 0;
    printf("vars\n");
	while (i < 2000){
    temp = temp + analog(0);
    msleep(1);
    i++;
    }
    printf("looped\n");
    int printer = temp/2000;
    printf("reading : %d\n", printer);
}


void stopTuah (int speed, int fiddle_speed){
	while( (analog(left_ir) < tape)  &&  (analog(right_ir) < tape) ){
		mav(lm, speed);
        mav(rm, speed);		
		msleep(10);
	}
    
    //if left sensor hits tape first
    if ( (analog(left_ir) > tape)  &&  (analog(right_ir) < tape) ){
    	//Stop tuahs left
        while(analog(right_ir) < tape ){
            while(analog (left_ir) > tape ) {
                freeze(rm);
                mav(lm, -fiddle_speed);
                msleep(1);
            }
            while(analog(left_ir) < tape ) {
            mav(lm, fiddle_speed);
            mav(rm, fiddle_speed);	
            }
        }
    }
    //if right sensor hits tape first
    if ( (analog(left_ir) < tape)  &&  (analog(right_ir) > tape) ){
    	//Stop tuahs right
        while(analog(left_ir) < tape ){
            while(analog (right_ir) > tape ) {
                freeze(lm);
                mav(rm, -fiddle_speed);
                msleep(1);
            }
            while(analog(right_ir) < tape ) {
            mav(lm, fiddle_speed);
            mav(rm, fiddle_speed);	
            }
        }
    }
    ao();
}

void stopTuahBack (int speed, int fiddle_speed){
	while( (analog(back_left_ir) < tape)  &&  (analog(back_right_ir) < tape) ){
		mav(lm, -speed);
        mav(rm, -speed);		
		msleep(10);
	}
    
    //if left sensor hits tape first
    if ( (analog(back_left_ir) > tape)  &&  (analog(back_right_ir) < tape) ){
    	//Stop tuahs left
        while(analog(back_right_ir) < tape ){
            while(analog (back_left_ir) > tape ) {
                freeze(rm);
                mav(lm, fiddle_speed);
                msleep(1);
            }
            while(analog(back_left_ir) < tape ) {
            mav(lm, -fiddle_speed);
            mav(rm, -fiddle_speed);	
            }
        }
    }
    //if right sensor hits tape first
    if ( (analog(back_left_ir) < tape)  &&  (analog(back_right_ir) > tape) ){
    	//Stop tuahs right
        while(analog(back_left_ir) < tape ){
            while(analog (back_right_ir) > tape ) {
                freeze(lm);
                mav(rm, fiddle_speed);
                msleep(1);
            }
            while(analog(back_right_ir) < tape ) {
            mav(lm, -fiddle_speed);
            mav(rm, -fiddle_speed);	
            }
        }
    }
    ao();
}

void sweepWater(){
	set_servo_position(sweep,sweep_out);
	msleep(200);
    set_servo_position(sweep, sweep_in);
}

void grabPomA() {
    set_servos(bPomA,aPomA, cOpen, 2);
    set_servos(bPomA,aPomA, cClose, 2);
    drive(-1, 900, 6);	//moves after grabbing to position for a safe drop
    ao();
    set_servos(bDrop, aDrop, cOpen, 2);
    set_servos(bCenter, aCenter, cOpen, 2);
    drive(1, 900, 6);
    ao();
}

void grabPomB(){
    set_servos(bPomB,aPomB, cOpen, 2);
    set_servos(bPomB,aPomB, cClose, 2);
    set_servos(bDrop, aDrop, cOpen, 2);
    set_servos(bCenter, aCenter, cOpen, 2);
}

void grabBigCube() {
    set_servos(bBigCube, aBigCube, cOpen, 2); //aliging for yellow cube A
    set_servos(bBigCube, aBigCube, cBigCube, 2); // grabbing yellow cube A
    set_servos(bDrop, aDrop, cOpen, 2);
    set_servos(bCenter, aCenter, cOpen, 2);
}
void grabBigCube2() {
    set_servos(bBigCube, aBigCube, cOpen, 2); //aliging for yellow cube A
    set_servos(bBigCube, aBigCube, cBigCube, 2);// grabbing yellow cube A
    drive(-1, 500, 4);
    set_servos(bDrop, aDrop, cOpen, 2);
    set_servos(bCenter, aCenter, cOpen, 2);
}
void grabCube() {
    set_servos(bCube,aCube, cOpen, 2);
    set_servos(bPomA,aPomA, cCube, 2);
    set_servos(bDrop, aDrop, cOpen, 2);
    set_servos(bCenter, aCenter, cOpen, 2);
}

void start_servos()
{
    enable_servos();
    set_servo_position(base, realBase);
    set_servo_position(arm, realArm);
    set_servo_position(claw, realClaw);
    msleep(200);
    disable_servos();
    enable_servos(sweep);
    set_servo_position(sweep, realSweep);
}

void center_arm() {
	set_servos(bCenter, aCenter, cOpen, 1);
}

int sweepn(int sweepPos, int delay) {
	//guard clause, if base, arm, or claw is too small or large, function exits with -1
    if (sweepPos < 50 || sweepPos > 1600) return -1;
   
    //initializing
    int sweepD = 0;
    int sweepDiff = sweepPos - realSweep;
   
    if (sweepDiff < 0)
    {
        sweepD = -2;
    }
    else
    {
        sweepD = 2;
    }
   
   
    //Move base and arm first
    enable_servo(sweep);
	
    //increments sweep by its direction
    while (realSweep != sweepPos)
    {
        if (realSweep != sweepPos) {
            realSweep = realSweep + sweepD;
            set_servo_position(sweep, realSweep);
        }
        msleep(delay);
    }
    
    return 1;
}
        
int set_servos (int basePos, int armPos, int clawPos, double delay)
{
    //guard clause, if base, arm, or claw is too small or large, function exits with -1
    if (basePos < 50 || basePos > 2000 || armPos < 50 || armPos > 2000 || clawPos < 50 || clawPos > 2000) return -1;
   
    //initializing
    int baseD;
    int armD;
    int baseDiff = basePos - realBase;
    int armDiff = armPos - realArm;
   
    if (baseDiff < 0)
    {
        baseD = -2;
    }
    else
    {
        baseD = 2;
    }
    if (armDiff < 0)
    {
        armD = -2;
    }
    else
    {
        armD = 2;
    }
   
   
    //Move base and arm first
    enable_servo(base);
    enable_servo(arm);

    while (realBase != basePos || realArm != armPos)
    {
        if (realBase != basePos) {
            realBase = realBase + baseD;
            set_servo_position(base, realBase);
        }
        if (realArm != armPos) {
            realArm = realArm + armD;
            set_servo_position(arm, realArm);
        }


        msleep(delay);
    }
    
    //Move claw instantly
    enable_servo(claw);
    set_servo_position(claw, clawPos);
    msleep(200);
    disable_servo(base);
    disable_servo(arm);
   
    return 0;
}