Pombot v. 3.0

// Created on Wed March 25 2015
// Pombot 2015; AKA; goldDIGGER; Written and Built by: The Freshman Team 2018

//sensor
#define BUTTON 15
//motors
#define RIGHTWHEEL 3
#define LEFTWHEEL 1
#define SORTER 0
//mini-servos
#define SWITCH 0
#define TAP 0
//servos
#define CLAW 2
#define ARM 1
//channels
#define RED 0
#define GREEN 1

//unneeded?
const int SWITCH_FRONT = 1030;
const int SWITCH_BACK = 1;
//arm position
const int ARM_UP = 2047;
const int ARM_MID = 1320;
const int ARM_DOWN = 200;
//claw position
const int CLAW_CLOSE = 900;
const int CLAW_OPEN = 200;
//tapper position
const int TAP_UP = 1450;
const int TAP_DOWN = 700;
//motor position/speed
const int MOTOR_POS_RED = 110;
const int MOTOR_POS_GREEN = 125;
const int MOTOR_SPEED = 135;
//the mutipling factor
const double factor = 0.92;

//prepping the robot
void setup();
//drive towards pompoms
void drive_to_first_blob();
//sorting our pompoms
void sort();
//updating the camera when we need it
void camera_four();
//setting positions of the arm and the claw
void set_arm(int pos);
void set_claw(int pos);
//keeping the robot moving straight
void drive_direct(int left_speed, int right_speed);
//moving forward
void drive(int speed, int millis);
//stoping the goldDIGGER from its monetary advancements
void stop();
//stoping the robot when the claw touches a wall
void drive_until_button(int speed);
//raises arm slowly
void set_arm_inc(int pos);
//idk if we are using this but okay
void shake();
//collects all the pompoms in the claw
void collect();
//functions that make the pompoms in the funnel go to the sorter one by one
void tap_funnel();
//functions to sort each color
void redsort();
void greensort();

//our code
int main()
    {
        printf("Pombot, the main goldDIGGER");
        setup();
        sort();
            return 0;
    }

    void setup()
    {
			//turns on the servos
            enable_servo(ARM);
            enable_servo(CLAW);
            enable_servo(TAP);
			//clears all the motor counters
            clear_motor_position_counter(RIGHTWHEEL);
            clear_motor_position_counter(LEFTWHEEL);
            clear_motor_position_counter(SORTER);
			//prepares the arm/claw
            set_arm(ARM_DOWN);
            set_claw(CLAW_OPEN);
			//opens the camera
            camera_open_at_res(LOW_RES);
    }

    void drive_to_first_blob()
    {
			//move forward
            drive(100, 3000);
			//turning slightly, keeps robot running straight
            drive_direct(-100, 100);
            msleep(700);
            //stop function
			stop();
            //moving backwards
			drive(-100, 1000);
			//until button
            drive_until_button(75);
            msleep(200);
			//collect function
            collect();
    }

void sort()
{
		//creating an interger count
        int counter = 0;
        //infinite loop
		for(;;)
        {
				//if count is 3
                if(counter == 3){
						//taps funnel once regardless
                        tap_funnel();
						//resets counter
                        counter = 0;
                }
				//activating camera function
                camera_four();
				//sees red more than green and only sees red
                if(get_object_area(RED,0) > get_object_area(GREEN,0) && get_object_area(RED,0) > 100)
                {
						//perform red sorting
                        redsort();
                        msleep(100);
                }
				//sees green more than red and only sees green
                if(get_object_area(GREEN,0) > get_object_area(RED,0) && get_object_area(GREEN,0) > 20)
                {
						//perform green sorting
                        greensort();
                        msleep(100);
                }
				//error fix version 1
				//if it sees both red and green, then it gets rid of the pompom: AKA REDSORT
                if(get_object_area(RED,0) > 100 && get_object_area(GREEN,0) > 100)

				{
						//red sort
						redsort();
						msleep(100);
				}
						msleep(10);
				//after one sorting pattern has been done, increase the counter by one
                counter++;
        }
}

void redsort()
{
		//turns the motor on, at a specfic speed to a certain position
        mtp(0, MOTOR_SPEED, -1 * MOTOR_POS_RED);
		//stops motor from moving at all in its spot
        block_motor_done(SORTER);
		//clears the motor counter
        clear_motor_position_counter(SORTER);
		//turns the motor on, at a specfic speed to the opp. position of the first
        mtp(0, MOTOR_SPEED, MOTOR_POS_RED);
		//stops motor from moving at all in its spot
        block_motor_done(SORTER);
		//clears the motor counter
        clear_motor_position_counter(SORTER);
}

void greensort()
{
		//turns the motor on, at a specfic speed to a certain position
		//change k
        mtp(0,MOTOR_SPEED, MOTOR_POS_GREEN);
		//stops motor from moving at all in its spot
        block_motor_done(SORTER);
		//clears the motor counter
        clear_motor_position_counter(SORTER);
		//turns the motor on, at a specfic speed to the opp. position of the first
        mtp(0,MOTOR_SPEED,-1 * MOTOR_POS_GREEN);
		//stops motor from moving at all in its spot
        block_motor_done(SORTER);
		//clears the motor counter
        clear_motor_position_counter(SORTER);
}

void camera_four()
{
		//update the camera 4 times when it's used
        camera_update();
        camera_update();
        camera_update();
        camera_update();
}

void set_arm(int pos)
    {
			//setting the arm pos.
            set_servo_position(ARM, pos);
    }

void set_claw(int pos)
    {
			//setting the claw pos.
            set_servo_position(CLAW, pos);
    }

void set_tapper(int pos)
    {
			//setting the tapper position
            set_servo_position(TAP, pos);
    }

void drive_direct(int left_speed, int right_speed) {
			//stablizing the movement of the robot
            motor(RIGHTWHEEL, right_speed);
            motor(LEFTWHEEL, factor * left_speed);
    }

void drive(int speed, int millis)
    {
			//driving
            drive_direct(speed, speed);
            msleep(millis);
            stop();
    }

void stop()
    {
			//stopping the bot
            drive_direct(0,0);
    }

void drive_until_button(int speed)
    {
			//moving until button is pressed
            drive_direct(speed, speed);
            while( digital(BUTTON) == 0 ){
            }
            stop();
    }

void shake()
    {
			//Unknown Potentially Used/Unused
            set_claw(CLAW_OPEN);
            msleep(2000);
            int i;
            for( i = 0; i < 5; i++ ){
                    set_claw(CLAW_CLOSE);
                    msleep(500);
                    set_claw(CLAW_OPEN);
                    msleep(500);
            }
    }
void set_arm_inc(int pos)
    {
			//moving the arm slowly
            const int incre = 30;
            int nDiff = (get_servo_position(ARM) - pos) / incre;
            int i = incre;
            for(; i>=0; i--)
            {
                    set_arm(pos + i * nDiff);
                    msleep(100);
            }
    }
void collect()
    {
			//collecting the pompoms in the claw
            set_claw(CLAW_OPEN);
            drive_until_button(200);
            set_claw(CLAW_CLOSE);
            msleep(500);
            drive(-100, 500);
            set_arm_inc(ARM_UP);
            msleep(500);
            shake();
    }
void tap_funnel()
    {
			//sorting the pompoms 1by1 within the funnel
            set_tapper(TAP_UP);
            msleep(200);
            set_tapper(TAP_DOWN);
    }