fetchBot.cpp

#include <math.h>       /* sqrt */
//Constants//

// Coordinates

//AXIS Units Ticks
const int X_AXIS = 0;
const int Y_AXIS = 0;

//Motors
const int LEFT_MOTOR = 0;
const int RIGHT_MOTOR = 3;

//Velocity
const int DEFAULT_VEL = 100;
const int DEFAULT_TURN_VEL = 2; // DEFAULT_TURN_VEL must be > 1

//Camera
const int BLUE_CHANNEL = 0;
const int RED_CHANNEL = 1;

//IRs
const int LIR_PIN = )3;
const int RIR_PIN = 4;

//room logic
const int roomCenter;
const string botMode = 'mapRoom';
//Functions
void avoid_foriegn_object(BLUE_CHANNEL);

//Main  //
int main() {

	printf("Pre opening \n");
	camera_open(LOW_RES);
	printf("camera opened \n");

	//Resolution variable
	int res = 160;

	while (1) {
		mode_handler();
	}
	return 0;
};

//Methods//
void mode_handler() {

	while (botMode == 'mapRoom') {
		map_room();
	};

	while (botMode == 'capturePuck') {
		capture_puck(RED_CHANNEL, res);
	};

};

//turn Right when
void turn_by_degree(int degree) {
	//compute the angle based upon the motor size (check and see what speed is best for turning (must be greater than 1.0))
	if (degree < 0) {
		DEFAULT_TURN_VEL = DEFAULT_TURN_VEL * -1
	};
	mav(LEFT_MOTOR, DEFAULT_VEL / DEFAULT_TURN_VEL);
	mav(RIGHT_MOTOR, DEFAULT_VEL * DEFAULT_TURN_VEL);
	//get_motor_position_counter - holder for total tics traveled
	int LEFT_MOTOR_TICKS = get_motor_position_counter(LEFT_MOTOR);
	int RIGHT_MOTOR_TICKS = get_motor_position_counter(RIGHT_MOTOR);
	int TICK_DIFFERNCE = LEFT_MOTOR_TICKS - RIGHT_MOTOR_TICKS;

	int TARGET_ANGLE_TO_TICKS = '?';//figure this out (how many tics to detmine a specfic angle)

	if ( TICK_DIFFERNCE == TARGET_ANGLE_TO_TICKS ) {
		ao();
		cruise_straight();
	}

};
//cruise straight
void cruise_straight() {
	mav(LEFT_MOTOR, DEFAULT_VEL)
	mav(RIGHT_MOTOR, DEFAULT_VEL)
}
/*Halts robot and polls sensors. Sensor pin is the input pin. resistor should be either 0 or 1
0 is no resistor, 1 is resistor. 0: IR proximity. 1: Light, IR beacon, Bump (analog).
	Change else statement
*/
void sensor_poll(int sensorPin, int resistor) {
	if (resistor)
		return analog10(sensorPin);
	else
		return analog_et(sensorPin);
	//return analog10(sensorPin);
}

//Provide the channel (with defined color) and whether you want x or y coordinate
void camera_poll(int channel, char axis) {
	motor(LEFT_MOTOR, 0);
	motor(RIGHT_MOTOR, 0);

	int val;
	camera_update();
	sleep(2);

	if (axis == 'x') {
		val = get_object_center(channel, 0).x;
		printf("%d \n", val);
		return val;
	}
	else {
		val = get_object_center(channel, 0).y;
		printf("%d \n", val);
		return val;
	}
}

//Provided with a value and a range 0-max. Outputs a value from 0-10, depending on degree of
// offset
void scale_input_for_motor(int val, int max) {
	int scaler = max / 10;
	val = val / scaler;
	return val;
}

//Angle is from 0 to 10
void turn_by_bin(int angle) {
	float lMotor = angle / 10.0;
	//float rMotor = 1.0 - lMotor;

	motor(LEFT_MOTOR, TOP_SPEED * lMotor);
	motor(RIGHT_MOTOR, TOP_SPEED * .5);
}

//Only works with low res,
void capture_puck(int channel, int res) {

	//Variable for x pos
	int xVal = camera_poll(channel, 'x');

	//printf("%d \n", xVal);

	Y_AXIS = mav(LEFT_MOTOR, <vel>);
	X_AXIS = mav(RIGHT_MOTOR, <vel>);

	if (xVal < 0) {
		cruise_straight();
	} else {
		//if Puck exits
		//Decide direction and angle to go
		int angle = scale_input_for_motor(xVal, res);

		turn_by_bin(angle);
	}
	sleep(1);
}

void map_room() {

	int cameraSensorViewLength = 'cameraDetectionRange'// need to find this
	                             int movesFromCenter = 0;

	const struct roomData
	{
		int wallCt = 0;
		int wall1Ticks; // first wall on right right side
		int wall2Ticks; // second wall on right side
	};

	//Setting the L && R IR pin location for sensor_poll
	const struct IR
	{
		int left = sensor_poll(LIR_PIN, 3);
		int right = sensor_poll(RIR_PIN, 4);
	};

	int leftMotorTicks = get_motor_position_counter(LEFT_MOTOR);

	if (IR.left <= 255 ) {
		if (roomData.wallCt == 0) {
			roomData.wall1Ticks = leftMotorTicks;
			turn_by_degree(-90);
		} else if (roomData.wallCt == 1) {
			roomData.wall2Ticks = leftMotorTicks - roomData.wall1Ticks;
			roomCenter = sqrt((roomData.wall1Ticks ^ 2) + (roomData.wall2Ticks ^ 2)) / 2;
			turn_by_degree(-125);
			cruise_straight(roomCenter);
		} else {
			return 0;
		}
		roomData.wallCt++;
	}

	if (leftMotorTicks == roomCenter) {
		turn_by_degree(-45);
		loop_room(cameraSensorViewLength, movesFromCenter);
	};

}

void loop_room(cameraSensorViewLength, movesFromCenter) {
	movesFromCenter++
	// Modulos => Increment movement by 1 unit every 2 moves (ie, 112233445 = 123456789)
	int multiplier = ((movesFromCenter + (movesFromCenter % 2)) / 2);
	int expandingMovementRange = multiplier * cameraSensorViewLength;

	cruise_straight(expandingMovementRange);
	turn_by_degree(-90);

	if (botMode == 'mapRoom') {
		loop_room();
	};

}

// camera y range total = 120
// drone delta = 120 <=> 80 (not included in 3 ranges of buffer)
// range1 = closestBuffer (danger) = 79 <=> 60
// range2 = middleBuffer = 59 <=> 21
// range3 = furthestBuffer = 0 <=> 20

void handle_bot_state(pd, orient, dir) {
	// dir true == parallel / false == perpendicular
	switch dir {
		case 'left':
			orient = (orient == 1) ? 4 : orient - 1
			break;
		case 'right':
			orient = (orient % 4) + 1
			break;
	}

	int inc = (orient == 1 || orient == 2) ? 1 : -1;

	if (orient == 1 || orient == 3) {
		pd.para = pd.para + inc;
	} else {
		pd.perp = pd.perp + inc;
	}

};

void avoid_when_obstructed(PathDelta, botDirection) {
	// if bot is faceing object at 60 unit turn right
	freeze();
	turn_by_degree(90);
	handle_bot_state(PathDelta, botDirection, 'right');
	cruise_straight(cameraSensorViewLength);
};

void correct_path_when_object_clear(PathDelta, botDirection) {
	// if bot off and no object in range turn left
	freeze();
	//subtract (get_motor_position_counter(LEFT_MOTOR);) from cameraSensorViewLength (this is currently a
	//private function)
	turn_by_degree(-90);
	handle_bot_state(PathDelta, botDirection, 'left');
}

void turn_back_to_path() {
	freeze();
	turn_by_degree(90);
}

void avoid_foriegn_object(int channel, int res) {

	int range1 = 60;
	int range2 = 21;
	int range3 = 0;

	int botDirection = 1;

	struct PathDelta = {
		int perp = 0;
		int para = 0;
	};

	camera_update();
	int object_right_side = get_object_center(channel, <number>).y; //??? WHAT IS NUMBER HERE ???

	if (object_right_side >= range1) {
		avoid_when_obstructed(PathDelta, botDirection)
	} else if (object_right_side < range3 && PathDelta.perp > 0) { //Check to ensure if there's no object, it's (<0)
		correct_path_when_object_clear(PathDelta, botDirection);
	} else if ( PathDelta.perp == 0) {
		turn_back_to_path();
	};

};