theRobot3 - uyvug

UH Robotics » Code » theRobot3

uyvug

Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos

main.cpp@33:a41981e18a7d, 2014-04-22 (annotated)

Committer:: Fairy_Paolina
Date:: Tue Apr 22 15:24:56 2014 +0000
Revision:: 33:a41981e18a7d
Parent:: 32:4a42f61f64a6
Child:: 34:cd4980573159

added derivative and Integral constants to PID. ; fixed align with gap,; align with rig;

Who changed what in which revision?

User	Revision	Line number	New contents of line
tashworth	0:1b64a0cedc5d	1	#include "mbed.h"
tashworth	0:1b64a0cedc5d	2	#include "Adafruit_PWMServoDriver.h"
tashworth	0:1b64a0cedc5d	3	#include "ShapeDetect.h"
tashworth	8:77a57909aa15	4	#include "rtos.h"
tashworth	8:77a57909aa15	5	#include "PID.h"
tashworth	8:77a57909aa15	6	#include "PololuQik2.h"
tashworth	8:77a57909aa15	7	#include "QEI.h"
tashworth	8:77a57909aa15	8	#include "HCSR04.h"
tashworth	8:77a57909aa15	9	#include "stdio.h"
tashworth	8:77a57909aa15	10	#include "LPC17xx.h"
tashworth	8:77a57909aa15	11	#include "Sharp.h"
tashworth	28:2bb6b0fe39d0	12
tashworth	28:2bb6b0fe39d0	13
tashworth	20:55dcff40c5d9	14	#define PI 3.14159
tashworth	28:2bb6b0fe39d0	15
tashworth	28:2bb6b0fe39d0	16
tashworth	28:2bb6b0fe39d0	17
tashworth	8:77a57909aa15	18	/* Navigation Definitions */
tashworth	8:77a57909aa15	19	#define PIN_TRIGGERL (p12)
tashworth	8:77a57909aa15	20	#define PIN_ECHOL (p11)
tashworth	8:77a57909aa15	21	#define PIN_TRIGGERR (p29)
tashworth	8:77a57909aa15	22	#define PIN_ECHOR (p30)
tashworth	8:77a57909aa15	23	#define PULSE_PER_REV (1192)
tashworth	8:77a57909aa15	24	#define WHEEL_CIRCUM (12.56637)
tashworth	8:77a57909aa15	25	#define DIST_PER_PULSE (0.01054225722682)
tashworth	8:77a57909aa15	26	#define MTRS_TO_INCH (39.3701)
tashworth	8:77a57909aa15	27	#define MAX_SPEED (0.3*127)
tashworth	8:77a57909aa15	28	#define PPR (4331/4)
tashworth	8:77a57909aa15	29	#define LEFT (1)
tashworth	8:77a57909aa15	30	#define RIGHT (0)
tashworth	12:284be46593ae	31	#define STRAIGHT (2)
tashworth	8:77a57909aa15	32	#define FORWARD (1)
tashworth	8:77a57909aa15	33	#define BACKWARD (0)
tashworth	8:77a57909aa15	34	#define TOOLS (0)
tashworth	8:77a57909aa15	35	#define MID (1)
tashworth	8:77a57909aa15	36	#define RIGS (2)
tashworth	12:284be46593ae	37	#define MID2 (3)
tashworth	12:284be46593ae	38	#define RETURN (4)
tashworth	8:77a57909aa15	39	#define FAR (1)
tashworth	28:2bb6b0fe39d0	40
tashworth	28:2bb6b0fe39d0	41
tashworth	6:75259c3306dd	42	//States
tashworth	6:75259c3306dd	43	#define START 0
tashworth	6:75259c3306dd	44	#define OILRIG1_POS 1
tashworth	6:75259c3306dd	45	#define OILRIG2_POS 2
tashworth	12:284be46593ae	46	#define GOTO_TOOLS1 3
tashworth	6:75259c3306dd	47	#define IDENTIFY_TOOLS 4
tashworth	6:75259c3306dd	48	#define AQUIRE_TOOL1 5
tashworth	6:75259c3306dd	49	#define AQUIRE_TOOL2 6
tashworth	6:75259c3306dd	50	#define AQUIRE_TOOL3 7
tashworth	6:75259c3306dd	51	#define NAVIGATE_WAVES_ROW1 8
tashworth	6:75259c3306dd	52	#define NAVIGATE_WAVES_ROW2 9
tashworth	6:75259c3306dd	53	#define NAVIGATE_WAVES_ROW3 10
tashworth	6:75259c3306dd	54	#define NAVIGATE_TO_SQUARE_RIG 11
tashworth	6:75259c3306dd	55	#define NAVIGATE_TO_TRIANGLE_RIG 12
tashworth	6:75259c3306dd	56	#define NAVIGATE_TO_CIRCLE_RIG 13
tashworth	6:75259c3306dd	57	#define RIG_ALIGN 14
tashworth	6:75259c3306dd	58	#define INSERT_TOOL 15
tashworth	6:75259c3306dd	59	#define END 16
tashworth	12:284be46593ae	60	#define GOTO_TOOLS2 17
tashworth	22:79c5871543b5	61	#define RETURN_TO_START 18
tashworth	28:2bb6b0fe39d0	62
tashworth	28:2bb6b0fe39d0	63
tashworth	28:2bb6b0fe39d0	64
tashworth	6:75259c3306dd	65	//Servo Static Positions
tashworth	6:75259c3306dd	66	#define STORE_POSITION 0
tashworth	6:75259c3306dd	67	#define OIL_RIG1 1
tashworth	6:75259c3306dd	68	#define OIL_RIG2 2
tashworth	6:75259c3306dd	69	#define OIL_RIG3 3
tashworth	6:75259c3306dd	70	#define DRIVE_POSITION_NOTOOL 4
tashworth	6:75259c3306dd	71	#define TOOL_1 5
tashworth	6:75259c3306dd	72	#define TOOL_2 6
tashworth	6:75259c3306dd	73	#define TOOL_3 7
tashworth	6:75259c3306dd	74	#define DRIVE_POSITION_TOOL 8
tashworth	6:75259c3306dd	75	#define ORIENT_TOOL 9
tashworth	29:22b243e288c8	76	#define PU_TOOL_1 10
tashworth	29:22b243e288c8	77	#define PU_TOOL_2 11
tashworth	29:22b243e288c8	78	#define PU_TOOL_3 12
tashworth	29:22b243e288c8	79	#define INSERT_TOOL_1 13
tashworth	28:2bb6b0fe39d0	80
tashworth	28:2bb6b0fe39d0	81
tashworth	6:75259c3306dd	82	//Rig definitions
tashworth	6:75259c3306dd	83	#define SQUARE 1
tashworth	6:75259c3306dd	84	#define TRIANGLE 2
tashworth	6:75259c3306dd	85	#define CIRCLE 3
tashworth	28:2bb6b0fe39d0	86
tashworth	28:2bb6b0fe39d0	87
tashworth	6:75259c3306dd	88	//Oil Rig distance thresholds
tashworth	21:0907e1f5e16c	89	#define OILRIG1_MAX 1800
tashworth	8:77a57909aa15	90	#define OILRIG1_MIN 1000
tashworth	21:0907e1f5e16c	91	#define OILRIG2_MAX 1800
tashworth	8:77a57909aa15	92	#define OILRIG2_MIN 1000
tashworth	21:0907e1f5e16c	93	#define OILRIG3_MAX 1800
tashworth	8:77a57909aa15	94	#define OILRIG3_MIN 1000
tashworth	28:2bb6b0fe39d0	95
tashworth	6:75259c3306dd	96	//for servo normalization
tashworth	3:b7b4780a7f6e	97	#define MIN_SERVO_PULSE 900
tashworth	3:b7b4780a7f6e	98	#define MAX_SERVO_PULSE 2100
tashworth	3:b7b4780a7f6e	99	#define SERVO_MAX_ANGLE 180
tashworth	28:2bb6b0fe39d0	100
tashworth	17:a5bb85ee205d	101	#define X_CENTER 80
tashworth	17:a5bb85ee205d	102	#define Y_CENTER 60
tashworth	28:2bb6b0fe39d0	103
tashworth	28:2bb6b0fe39d0	104
tashworth	8:77a57909aa15	105	DigitalOut myled1(LED1);
tashworth	8:77a57909aa15	106	DigitalOut myled2(LED2);
tashworth	8:77a57909aa15	107	DigitalOut myled3(LED3);
tashworth	8:77a57909aa15	108	DigitalOut myled4(LED4);
tashworth	16:8bb212df81b7	109	InterruptIn startBtn(p7);
tashworth	28:2bb6b0fe39d0	110
tashworth	8:77a57909aa15	111	void errFunction(void);
tashworth	8:77a57909aa15	112	bool cRc;
tashworth	28:2bb6b0fe39d0	113
tashworth	8:77a57909aa15	114	Serial pc(USBTX,USBRX); //USB Comm
tashworth	8:77a57909aa15	115	Adafruit_PWMServoDriver pwm(p28,p27); //pwm(SDA,SCL) - Servo Control PWM
tashworth	8:77a57909aa15	116	extern Serial lrf; //Laser Range Finder lrf(p13,p14)
tashworth	8:77a57909aa15	117	//Hardware Initialization
tashworth	8:77a57909aa15	118	//Serial bt(p13,p14);
Fairy_Paolina	27:5540aa3c828c	119	HCSR04 rangeFinderFront( PIN_TRIGGERL, PIN_ECHOL );
tashworth	8:77a57909aa15	120	HCSR04 rangeFinderRight( PIN_TRIGGERR, PIN_ECHOR );
Fairy_Paolina	33:a41981e18a7d	121	PID pid1(25.0,2.0,1.0,0.0001);
Fairy_Paolina	30:db07aea6d119	122	PololuQik2 motors(p9, p10, p8, p11, &errFunction, cRc);
Fairy_Paolina	30:db07aea6d119	123	QEI rightEncoder(p22,p21,NC,PPR,QEI::X4_ENCODING);
Fairy_Paolina	30:db07aea6d119	124	QEI leftEncoder(p23,p24,NC,PPR,QEI::X4_ENCODING);
Fairy_Paolina	30:db07aea6d119	125	Sharp IRLeftFront(p18);
Fairy_Paolina	30:db07aea6d119	126	Sharp IRLeftBack(p17);
Fairy_Paolina	30:db07aea6d119	127	Sharp IRRightFront(p20);
Fairy_Paolina	30:db07aea6d119	128	Sharp IRRightBack(p15);
Fairy_Paolina	30:db07aea6d119	129	Sharp IRFrontL(p19);
Fairy_Paolina	30:db07aea6d119	130	Sharp IRFrontR(p16);
tashworth	8:77a57909aa15	131	//InterruptIn encoder(p29);
tashworth	28:2bb6b0fe39d0	132
tashworth	28:2bb6b0fe39d0	133
tashworth	28:2bb6b0fe39d0	134
tashworth	28:2bb6b0fe39d0	135
tashworth	6:75259c3306dd	136	/***************
tashworth	6:75259c3306dd	137	local servo functions
tashworth	6:75259c3306dd	138	****************/
tashworth	1:fe4a0b47ff25	139	void servoBegin(void);
tashworth	0:1b64a0cedc5d	140	void initServoDriver(void);
tashworth	3:b7b4780a7f6e	141	void setServoPulse(uint8_t n, int angle);
tashworth	17:a5bb85ee205d	142	void setServoPulse2(uint8_t n, float angle); //float precision
tashworth	3:b7b4780a7f6e	143	void setServoPulseNo_delay(uint8_t n, int angle);
tashworth	0:1b64a0cedc5d	144	void servoPosition(int set);
tashworth	6:75259c3306dd	145	int fire_checker(int rig);
tashworth	16:8bb212df81b7	146	int button_start = 0;
tashworth	28:2bb6b0fe39d0	147
tashworth	28:2bb6b0fe39d0	148
tashworth	8:77a57909aa15	149	//Navigation Functions
tashworth	12:284be46593ae	150	void wall_follow2(int side, int direction, int section, float location, int rig);
tashworth	8:77a57909aa15	151	void leftTurn(void);
tashworth	8:77a57909aa15	152	void slightleft(void);
tashworth	12:284be46593ae	153	void slightright(void);
tashworth	8:77a57909aa15	154	void rightTurn(void);
tashworth	12:284be46593ae	155	void slightMove(int direction, float pulses);
Fairy_Paolina	27:5540aa3c828c	156	void tools_section_return(float* location, float &current);
tashworth	12:284be46593ae	157	void to_tools_section1(float* location, float &current);
tashworth	12:284be46593ae	158	void to_tools_section2(float* location, float &current);
tashworth	8:77a57909aa15	159	void from_tools_section(float* location, float &current);
tashworth	8:77a57909aa15	160	void mid_section(float* location, float &current, int* direction);
tashworth	8:77a57909aa15	161	void mid_section2(float* location, float &current, int* direction);
tashworth	8:77a57909aa15	162	void rig_section(float* location, float &current, int* direction, int rig);
tashworth	12:284be46593ae	163	void mid_section_return(float* location, float &current, int* direction);
tashworth	12:284be46593ae	164	void mid_section2_return(float* location, float &current, int* direction);
tashworth	12:284be46593ae	165	void rig_section_return(float* location, float &current, int* direction);
tashworth	8:77a57909aa15	166	void overBump(int section);
tashworth	8:77a57909aa15	167	void alignWithWall(int section);
tashworth	12:284be46593ae	168	void UntilWall(int dir);
Fairy_Paolina	33:a41981e18a7d	169	void alignWithGap(void);
Fairy_Paolina	33:a41981e18a7d	170	void alignWithRig(void);
tashworth	28:2bb6b0fe39d0	171
tashworth	28:2bb6b0fe39d0	172	void testSensors(void);
tashworth	20:55dcff40c5d9	173	float normd(int* pop, int count, int threshold);
tashworth	20:55dcff40c5d9	174	int Xadjust(int tool);
tashworth	28:2bb6b0fe39d0	175
tashworth	16:8bb212df81b7	176	extern "C" void mbed_reset();
tashworth	28:2bb6b0fe39d0	177
tashworth	6:75259c3306dd	178	/************
tashworth	6:75259c3306dd	179	Main Variables
tashworth	6:75259c3306dd	180	*************/
tashworth	6:75259c3306dd	181	int state = START;
tashworth	6:75259c3306dd	182	int fire = 0;
tashworth	20:55dcff40c5d9	183	int tool_needed = 1;
tashworth	6:75259c3306dd	184	int shape_detected = 0;
Fairy_Paolina	27:5540aa3c828c	185	float range, range2, range3, pid_return;
tashworth	19:d4d967a885dc	186	int num, input;
tashworth	28:2bb6b0fe39d0	187
tashworth	28:2bb6b0fe39d0	188
tashworth	0:1b64a0cedc5d	189	/************
tashworth	0:1b64a0cedc5d	190	Variables for Servos
tashworth	0:1b64a0cedc5d	191	*************/
tashworth	3:b7b4780a7f6e	192	int servoNum, servoAngle, outputDisabled, posNum, testVal;
tashworth	3:b7b4780a7f6e	193	int currentPosition[7];
tashworth	28:2bb6b0fe39d0	194
tashworth	3:b7b4780a7f6e	195	typedef struct {
tashworth	6:75259c3306dd	196	int arm_position_name; //for organization only (STORE, OILRIG1, OILRIG2...)
tashworth	3:b7b4780a7f6e	197	int base_rotate;
tashworth	3:b7b4780a7f6e	198	int base_arm;
tashworth	3:b7b4780a7f6e	199	int big_arm;
tashworth	3:b7b4780a7f6e	200	int claw_arm;
tashworth	3:b7b4780a7f6e	201	int claw_rotate;
tashworth	3:b7b4780a7f6e	202	int claw_open;
tashworth	3:b7b4780a7f6e	203	} Coord;
tashworth	28:2bb6b0fe39d0	204
tashworth	6:75259c3306dd	205	/********************
tashworth	6:75259c3306dd	206	Static Arm Positions
tashworth	6:75259c3306dd	207	*********************/
tashworth	28:2bb6b0fe39d0	208
tashworth	3:b7b4780a7f6e	209	Coord Arm_Table[] = {
tashworth	28:2bb6b0fe39d0	210
tashworth	3:b7b4780a7f6e	211	// POSITION ODER:
tashworth	6:75259c3306dd	212	// base_rotate, base_arm, int big_arm, int claw_arm, int claw_rotate, int claw_open
tashworth	28:2bb6b0fe39d0	213
tashworth	8:77a57909aa15	214	//increase in number 5 rotates gripper
tashworth	28:2bb6b0fe39d0	215
srsmitherman	31:99cf9c25b0f2	216	{STORE_POSITION, 85, 8, 0, 170, 60, 0}, // storing position
jjcarr2	32:4a42f61f64a6	217	{OIL_RIG1, 158, 20, 61, 100, 175, 0}, // point laser at oilrig1
jjcarr2	32:4a42f61f64a6	218	{OIL_RIG2, 158, 20, 61, 100, 175, 0}, // point laser at oilrig2
tashworth	28:2bb6b0fe39d0	219	{OIL_RIG3, 130, 90, 90, 100, 175, 0}, // NOT USED!!!!! point laser at oilrig2
tashworth	28:2bb6b0fe39d0	220	{DRIVE_POSITION_NOTOOL, 85, 10, 0, 165, 60, 0}, // Drive through course
tashworth	28:2bb6b0fe39d0	221	{TOOL_1, 101, 50, 80, 133, 60, 0}, // Look over first tool
tashworth	28:2bb6b0fe39d0	222	{TOOL_2, 82, 50, 80, 133, 60, 0}, // Look over second tool
tashworth	28:2bb6b0fe39d0	223	{TOOL_3, 62, 50, 80, 132, 60, 0}, // Look over third tool
tashworth	28:2bb6b0fe39d0	224	{DRIVE_POSITION_TOOL, 85, 10, 0, 165, 140, 120}, // Drive with tool loaded
tashworth	6:75259c3306dd	225	{ORIENT_TOOL, 135, 60, 75, 60, 90, 90}, // position tool to be inserted
jjcarr2	32:4a42f61f64a6	226	{PU_TOOL_1, 99, 46, 78, 110, 138, 0}, // STATIC Pickup tool POSITION
jjcarr2	32:4a42f61f64a6	227	{PU_TOOL_2, 76, 47, 80, 112, 138, 0}, // STATIC Pickup tool POSITION
jjcarr2	32:4a42f61f64a6	228	{PU_TOOL_3, 59, 44, 76, 110, 138, 0}, // STATIC Pickup tool POSITION
tashworth	28:2bb6b0fe39d0	229	{INSERT_TOOL_1, 170, 50, 127, 52, 140, 120}, // InsertToolIntoRig
tashworth	28:2bb6b0fe39d0	230
tashworth	3:b7b4780a7f6e	231	};
tashworth	28:2bb6b0fe39d0	232
tashworth	28:2bb6b0fe39d0	233
tashworth	3:b7b4780a7f6e	234	/* Variables for imageprocessing and distance */
tashworth	3:b7b4780a7f6e	235	int x_coord;
tashworth	3:b7b4780a7f6e	236	int y_coord;
tashworth	3:b7b4780a7f6e	237	int area;
tashworth	3:b7b4780a7f6e	238	int shape;
tashworth	13:529323807361	239	int shape_alignX_done = 0;
tashworth	16:8bb212df81b7	240	int shape_alignY_done = 0;
tashworth	17:a5bb85ee205d	241	float deltaX = 0;
tashworth	28:2bb6b0fe39d0	242
tashworth	28:2bb6b0fe39d0	243
tashworth	6:75259c3306dd	244	/* Variables for distance sensor*/
tashworth	8:77a57909aa15	245	int distLaser;
tashworth	6:75259c3306dd	246	int fire_detected = 0;
tashworth	6:75259c3306dd	247	int fire_not_detected = 0;
tashworth	28:2bb6b0fe39d0	248
tashworth	16:8bb212df81b7	249	void button_int(void)
tashworth	16:8bb212df81b7	250	{
tashworth	16:8bb212df81b7	251	if(!button_start) {
tashworth	16:8bb212df81b7	252	button_start = 1;
tashworth	16:8bb212df81b7	253	wait(1.0);
tashworth	16:8bb212df81b7	254	} else {
tashworth	16:8bb212df81b7	255	button_start = 0;
tashworth	16:8bb212df81b7	256	mbed_reset();
tashworth	16:8bb212df81b7	257	}
tashworth	16:8bb212df81b7	258	return;
tashworth	16:8bb212df81b7	259	}
tashworth	28:2bb6b0fe39d0	260
tashworth	28:2bb6b0fe39d0	261
tashworth	28:2bb6b0fe39d0	262
tashworth	3:b7b4780a7f6e	263	int main()
tashworth	3:b7b4780a7f6e	264	{
tashworth	28:2bb6b0fe39d0	265
tashworth	28:2bb6b0fe39d0	266
tashworth	3:b7b4780a7f6e	267	/*****************
tashworth	3:b7b4780a7f6e	268	INITIALIZATIONS
tashworth	3:b7b4780a7f6e	269	*******************/
tashworth	12:284be46593ae	270	float location[3], current=4;
tashworth	8:77a57909aa15	271	int direction[3];
tashworth	8:77a57909aa15	272	double distance;
tashworth	28:2bb6b0fe39d0	273
tashworth	17:a5bb85ee205d	274	int pu, num, input;
tashworth	28:2bb6b0fe39d0	275
tashworth	28:2bb6b0fe39d0	276
tashworth	3:b7b4780a7f6e	277	pc.baud(115200);
tashworth	28:2bb6b0fe39d0	278
tashworth	6:75259c3306dd	279	//Laser Range Finder Initialization
tashworth	3:b7b4780a7f6e	280	lrf_baudCalibration();
tashworth	28:2bb6b0fe39d0	281
tashworth	8:77a57909aa15	282	motors.begin();
tashworth	28:2bb6b0fe39d0	283
tashworth	16:8bb212df81b7	284	startBtn.rise(&button_int);
srsmitherman	31:99cf9c25b0f2	285	//////////////////////////// TEST SERVOS /////////////////////////
Fairy_Paolina	30:db07aea6d119	286	//testSensors();
srsmitherman	31:99cf9c25b0f2	287
Fairy_Paolina	30:db07aea6d119	288	motors.begin();
Fairy_Paolina	30:db07aea6d119	289	//wall_follow2(LEFT, FORWARD, TOOLS, 10, 1);
Fairy_Paolina	30:db07aea6d119	290	//while(1);
tashworth	28:2bb6b0fe39d0	291
tashworth	6:75259c3306dd	292	//Servo initialization
tashworth	3:b7b4780a7f6e	293	initServoDriver();
tashworth	6:75259c3306dd	294	servoBegin(); //initiates servos to start position
tashworth	11:8d2455e383ce	295	//ServoOutputDisable = 0;
tashworth	28:2bb6b0fe39d0	296
tashworth	7:8fb4204f9600	297	/********************************
tashworth	7:8fb4204f9600	298	MAIN WHILE LOOP FOR COMPETITION
tashworth	7:8fb4204f9600	299	*********************************/
tashworth	28:2bb6b0fe39d0	300
tashworth	28:2bb6b0fe39d0	301
tashworth	3:b7b4780a7f6e	302	while(1) {
tashworth	28:2bb6b0fe39d0	303	//if(1) {
tashworth	16:8bb212df81b7	304	if(button_start == 1) {
srsmitherman	31:99cf9c25b0f2	305
tashworth	28:2bb6b0fe39d0	306	/*
tashworth	28:2bb6b0fe39d0	307	pc.printf("Servo Test");
tashworth	28:2bb6b0fe39d0	308	while(1) {
tashworth	28:2bb6b0fe39d0	309	pc.scanf("%d %d", &servoNum, &servoAngle);
tashworth	28:2bb6b0fe39d0	310	if(servoAngle > 175) {
tashworth	28:2bb6b0fe39d0	311	servoAngle = 175;
tashworth	28:2bb6b0fe39d0	312	}
tashworth	28:2bb6b0fe39d0	313	if(servoNum > 5 ) {
tashworth	28:2bb6b0fe39d0	314	servoNum = 0;
tashworth	28:2bb6b0fe39d0	315	servoAngle = 90;
tashworth	28:2bb6b0fe39d0	316	}
tashworth	28:2bb6b0fe39d0	317	setServoPulse(servoNum, servoAngle);
tashworth	28:2bb6b0fe39d0	318
tashworth	28:2bb6b0fe39d0	319	};*/
tashworth	28:2bb6b0fe39d0	320
tashworth	28:2bb6b0fe39d0	321
tashworth	16:8bb212df81b7	322	switch (state) {
tashworth	28:2bb6b0fe39d0	323
tashworth	16:8bb212df81b7	324	/**************************************************
tashworth	16:8bb212df81b7	325	* STAGE 0
tashworth	16:8bb212df81b7	326	*
tashworth	16:8bb212df81b7	327	* - START OF THE COMETITION
tashworth	16:8bb212df81b7	328	*
tashworth	16:8bb212df81b7	329	**************************************************/
tashworth	16:8bb212df81b7	330	case START :
tashworth	16:8bb212df81b7	331	myled1 = 1;
tashworth	28:2bb6b0fe39d0	332
Fairy_Paolina	33:a41981e18a7d	333	current=68;
Fairy_Paolina	33:a41981e18a7d	334	state = NAVIGATE_WAVES_ROW1;
Fairy_Paolina	33:a41981e18a7d	335	//state = OILRIG1_POS;
tashworth	16:8bb212df81b7	336	break;
tashworth	28:2bb6b0fe39d0	337
tashworth	28:2bb6b0fe39d0	338
tashworth	16:8bb212df81b7	339	/**************************************************
tashworth	16:8bb212df81b7	340	* STAGE 1
tashworth	16:8bb212df81b7	341	*
tashworth	16:8bb212df81b7	342	* - DETERMINE OIL RIG ON FIRE
tashworth	16:8bb212df81b7	343	*
tashworth	16:8bb212df81b7	344	**************************************************/
tashworth	28:2bb6b0fe39d0	345
tashworth	16:8bb212df81b7	346	case OILRIG1_POS: //aims arm at square oil rig
tashworth	28:2bb6b0fe39d0	347
tashworth	16:8bb212df81b7	348	servoPosition(OIL_RIG1);
tashworth	16:8bb212df81b7	349	wait(3); //wait for servo to settle before laser distance
tashworth	28:2bb6b0fe39d0	350
tashworth	16:8bb212df81b7	351	fire = fire_checker(OIL_RIG1); //determines if oil rig is on fire
tashworth	28:2bb6b0fe39d0	352
tashworth	16:8bb212df81b7	353	//determines what tool is needed
tashworth	16:8bb212df81b7	354	if (fire == 1) {
tashworth	16:8bb212df81b7	355	pc.printf("FIRE FOUND!!!!!!!!\n\r");
tashworth	16:8bb212df81b7	356	tool_needed = SQUARE;
tashworth	16:8bb212df81b7	357	state = GOTO_TOOLS1;
tashworth	16:8bb212df81b7	358	} else {
tashworth	16:8bb212df81b7	359	pc.printf("XXXXXX FIRE NOT FOUND XXXXXX");
tashworth	16:8bb212df81b7	360	state = OILRIG2_POS;
tashworth	16:8bb212df81b7	361	}
tashworth	16:8bb212df81b7	362	break;
tashworth	28:2bb6b0fe39d0	363
tashworth	16:8bb212df81b7	364	case OILRIG2_POS:
tashworth	28:2bb6b0fe39d0	365
tashworth	16:8bb212df81b7	366	setServoPulse(2, Arm_Table[DRIVE_POSITION_NOTOOL].big_arm);
tashworth	16:8bb212df81b7	367	setServoPulse(3, Arm_Table[DRIVE_POSITION_NOTOOL].claw_arm);
tashworth	16:8bb212df81b7	368	setServoPulse(1, Arm_Table[DRIVE_POSITION_NOTOOL].base_arm);
tashworth	16:8bb212df81b7	369	setServoPulse(0, Arm_Table[DRIVE_POSITION_NOTOOL].base_rotate);
tashworth	16:8bb212df81b7	370	setServoPulse(4, Arm_Table[DRIVE_POSITION_NOTOOL].claw_rotate);
tashworth	16:8bb212df81b7	371	setServoPulse(5, Arm_Table[DRIVE_POSITION_NOTOOL].claw_open);
tashworth	16:8bb212df81b7	372	wait(3); //wait for servos to settle
tashworth	28:2bb6b0fe39d0	373
tashworth	16:8bb212df81b7	374	to_tools_section2(location, current); // moves to second rig
tashworth	28:2bb6b0fe39d0	375
tashworth	16:8bb212df81b7	376	servoPosition(OIL_RIG2); //position arm to point at first oilrig
tashworth	16:8bb212df81b7	377	wait(3);
tashworth	28:2bb6b0fe39d0	378
tashworth	16:8bb212df81b7	379	fire = fire_checker(OIL_RIG2);
tashworth	16:8bb212df81b7	380	if (fire == 1) {
tashworth	16:8bb212df81b7	381	pc.printf("FIRE FOUND!!!!!!!!");
tashworth	16:8bb212df81b7	382	tool_needed = TRIANGLE;
tashworth	16:8bb212df81b7	383	state = GOTO_TOOLS2;
tashworth	16:8bb212df81b7	384	} else {
tashworth	16:8bb212df81b7	385	pc.printf("XXXXXX FIRE NOT FOUND XXXXXX");
tashworth	16:8bb212df81b7	386	tool_needed = CIRCLE;
tashworth	16:8bb212df81b7	387	state = GOTO_TOOLS2;
tashworth	16:8bb212df81b7	388	}
tashworth	16:8bb212df81b7	389	break;
tashworth	28:2bb6b0fe39d0	390
tashworth	16:8bb212df81b7	391	/**************************************************
tashworth	16:8bb212df81b7	392	* STAGE 2
tashworth	16:8bb212df81b7	393	*
tashworth	16:8bb212df81b7	394	* - TRAVEL TO TOOLS
tashworth	16:8bb212df81b7	395	*
tashworth	16:8bb212df81b7	396	**************************************************/
tashworth	16:8bb212df81b7	397	case GOTO_TOOLS1:
tashworth	16:8bb212df81b7	398	setServoPulse(2, Arm_Table[DRIVE_POSITION_NOTOOL].big_arm);
tashworth	16:8bb212df81b7	399	setServoPulse(3, Arm_Table[DRIVE_POSITION_NOTOOL].claw_arm);
tashworth	16:8bb212df81b7	400	setServoPulse(1, Arm_Table[DRIVE_POSITION_NOTOOL].base_arm);
tashworth	16:8bb212df81b7	401	setServoPulse(0, Arm_Table[DRIVE_POSITION_NOTOOL].base_rotate);
tashworth	16:8bb212df81b7	402	setServoPulse(4, Arm_Table[DRIVE_POSITION_NOTOOL].claw_rotate);
tashworth	16:8bb212df81b7	403	setServoPulse(5, Arm_Table[DRIVE_POSITION_NOTOOL].claw_open);
tashworth	16:8bb212df81b7	404	wait(3); //wait for servos to settle
tashworth	28:2bb6b0fe39d0	405
tashworth	16:8bb212df81b7	406	to_tools_section1(location, current);
tashworth	16:8bb212df81b7	407	state = IDENTIFY_TOOLS;
tashworth	16:8bb212df81b7	408	break;
tashworth	28:2bb6b0fe39d0	409
tashworth	16:8bb212df81b7	410	case GOTO_TOOLS2:
tashworth	28:2bb6b0fe39d0	411
tashworth	16:8bb212df81b7	412	setServoPulse(2, Arm_Table[DRIVE_POSITION_NOTOOL].big_arm);
tashworth	16:8bb212df81b7	413	setServoPulse(3, Arm_Table[DRIVE_POSITION_NOTOOL].claw_arm);
tashworth	16:8bb212df81b7	414	setServoPulse(1, Arm_Table[DRIVE_POSITION_NOTOOL].base_arm);
tashworth	16:8bb212df81b7	415	setServoPulse(0, Arm_Table[DRIVE_POSITION_NOTOOL].base_rotate);
tashworth	16:8bb212df81b7	416	setServoPulse(4, Arm_Table[DRIVE_POSITION_NOTOOL].claw_rotate);
tashworth	16:8bb212df81b7	417	setServoPulse(5, Arm_Table[DRIVE_POSITION_NOTOOL].claw_open);
tashworth	16:8bb212df81b7	418	wait(3); //wait for servos to settle
tashworth	28:2bb6b0fe39d0	419
tashworth	21:0907e1f5e16c	420	slightMove(FORWARD,3250);
tashworth	16:8bb212df81b7	421	current+=(abs(leftEncoder.getPulses()11.12/PPR) + abs(rightEncoder.getPulses()11.12/PPR))/2;
tashworth	28:2bb6b0fe39d0	422
tashworth	16:8bb212df81b7	423	state = IDENTIFY_TOOLS;
tashworth	16:8bb212df81b7	424	break;
tashworth	28:2bb6b0fe39d0	425
tashworth	28:2bb6b0fe39d0	426
tashworth	28:2bb6b0fe39d0	427
tashworth	16:8bb212df81b7	428	/**************************************************
tashworth	16:8bb212df81b7	429	* STAGE 3
tashworth	16:8bb212df81b7	430	*
tashworth	16:8bb212df81b7	431	* - Determine order of tools
tashworth	16:8bb212df81b7	432	* - Aquire appropriate tool
tashworth	16:8bb212df81b7	433	*
tashworth	16:8bb212df81b7	434	**************************************************/
tashworth	16:8bb212df81b7	435	case IDENTIFY_TOOLS:
Fairy_Paolina	33:a41981e18a7d	436
tashworth	20:55dcff40c5d9	437	//wait(5);
tashworth	16:8bb212df81b7	438	servoPosition(TOOL_2); //arm/camera looks over tool
tashworth	16:8bb212df81b7	439	wait(5); //wait for servos to settle
tashworth	28:2bb6b0fe39d0	440
tashworth	16:8bb212df81b7	441	//shape_detected = shapeDetection(); //determines the shape
tashworth	16:8bb212df81b7	442	//clearBounds();
tashworth	16:8bb212df81b7	443	//printImageToFile(BINARY);
tashworth	16:8bb212df81b7	444	shape_alignX_done = 0;
tashworth	16:8bb212df81b7	445	shape_alignY_done = 0;
tashworth	17:a5bb85ee205d	446	/*
tashworth	17:a5bb85ee205d	447	while( shape_alignY_done == 0) {
tashworth	17:a5bb85ee205d	448	wait(1);
tashworth	17:a5bb85ee205d	449	shape_detected = shapeDetection();
tashworth	28:2bb6b0fe39d0	450
tashworth	17:a5bb85ee205d	451	pc.printf("Y - Adjust to center tool\n\r");
tashworth	28:2bb6b0fe39d0	452
tashworth	17:a5bb85ee205d	453	if(get_com_y() < 50) {
tashworth	17:a5bb85ee205d	454	wait(1);
tashworth	17:a5bb85ee205d	455	slightMove(FORWARD,25);
tashworth	17:a5bb85ee205d	456	current-=(abs(leftEncoder.getPulses()11.12/PPR) + abs(rightEncoder.getPulses()11.12/PPR))/2;
tashworth	28:2bb6b0fe39d0	457
tashworth	17:a5bb85ee205d	458	} else if(get_com_y() > 70) {
tashworth	17:a5bb85ee205d	459	wait(1);
tashworth	17:a5bb85ee205d	460	slightMove(BACKWARD,25);
tashworth	17:a5bb85ee205d	461	current+=(abs(leftEncoder.getPulses()11.12/PPR) + abs(rightEncoder.getPulses()11.12/PPR))/2;
tashworth	28:2bb6b0fe39d0	462
tashworth	17:a5bb85ee205d	463	} else {
tashworth	17:a5bb85ee205d	464	shape_alignY_done = 1;
tashworth	17:a5bb85ee205d	465	}
tashworth	17:a5bb85ee205d	466	}*/
tashworth	28:2bb6b0fe39d0	467
tashworth	20:55dcff40c5d9	468	// aveArea = sumArea/count;
tashworth	17:a5bb85ee205d	469	//printImageToFile(BINARY);
tashworth	16:8bb212df81b7	470	//either goes to aquire the tool or look at the next shape
tashworth	28:2bb6b0fe39d0	471
tashworth	22:79c5871543b5	472	//****************//if(Xadjust(TOOL_2) == tool_needed) {
tashworth	22:79c5871543b5	473	if(Xadjust(TOOL_2) == 162) {
tashworth	20:55dcff40c5d9	474	//printImageToFile(BINARY);
tashworth	16:8bb212df81b7	475	state = AQUIRE_TOOL2;
tashworth	16:8bb212df81b7	476	break;
tashworth	13:529323807361	477	} else {
tashworth	20:55dcff40c5d9	478	//printImageToFile(BINARY);
tashworth	21:0907e1f5e16c	479	slightMove(FORWARD,70);
tashworth	16:8bb212df81b7	480	current+=(abs(leftEncoder.getPulses()11.12/PPR) + abs(rightEncoder.getPulses()11.12/PPR))/2;
tashworth	16:8bb212df81b7	481	servoPosition(TOOL_1);
tashworth	16:8bb212df81b7	482	wait(5); //wait for servos to settle
tashworth	16:8bb212df81b7	483	shape_alignX_done = 0;
tashworth	16:8bb212df81b7	484	shape_alignY_done = 0;
tashworth	17:a5bb85ee205d	485	/*
tashworth	16:8bb212df81b7	486	while( shape_alignY_done == 0) {
tashworth	16:8bb212df81b7	487	wait(1);
tashworth	16:8bb212df81b7	488	shape_detected = shapeDetection();
tashworth	28:2bb6b0fe39d0	489
tashworth	16:8bb212df81b7	490	pc.printf("Y - Adjust to center tool\n\r");
tashworth	28:2bb6b0fe39d0	491
tashworth	17:a5bb85ee205d	492	if(get_com_y() < 50) {
tashworth	16:8bb212df81b7	493	wait(1);
tashworth	16:8bb212df81b7	494	slightMove(FORWARD,25);
tashworth	16:8bb212df81b7	495	current-=(abs(leftEncoder.getPulses()11.12/PPR) + abs(rightEncoder.getPulses()11.12/PPR))/2;
tashworth	28:2bb6b0fe39d0	496
tashworth	17:a5bb85ee205d	497	} else if(get_com_y() > 70) {
tashworth	16:8bb212df81b7	498	wait(1);
tashworth	16:8bb212df81b7	499	slightMove(BACKWARD,25);
tashworth	16:8bb212df81b7	500	current+=(abs(leftEncoder.getPulses()11.12/PPR) + abs(rightEncoder.getPulses()11.12/PPR))/2;
tashworth	28:2bb6b0fe39d0	501
tashworth	16:8bb212df81b7	502	} else {
tashworth	16:8bb212df81b7	503	shape_alignY_done = 1;
tashworth	16:8bb212df81b7	504	}
tashworth	17:a5bb85ee205d	505	}*/
tashworth	22:79c5871543b5	506	//****************// if (Xadjust(TOOL_1) == tool_needed) {
tashworth	22:79c5871543b5	507	if (Xadjust(TOOL_1) == 169) {
tashworth	16:8bb212df81b7	508	state = AQUIRE_TOOL1;
tashworth	16:8bb212df81b7	509	break;
tashworth	16:8bb212df81b7	510	} else {
tashworth	16:8bb212df81b7	511	servoPosition(TOOL_3);
tashworth	16:8bb212df81b7	512	wait(3); //wait for servos to settle
tashworth	16:8bb212df81b7	513	state = AQUIRE_TOOL3;
tashworth	16:8bb212df81b7	514	}
tashworth	13:529323807361	515	}
tashworth	28:2bb6b0fe39d0	516
tashworth	16:8bb212df81b7	517	break;
tashworth	28:2bb6b0fe39d0	518
tashworth	16:8bb212df81b7	519	case AQUIRE_TOOL1:
tashworth	28:2bb6b0fe39d0	520
tashworth	16:8bb212df81b7	521	servoPosition(PU_TOOL_1);
tashworth	18:a0ea7ecaf4fe	522	setServoPulse(4, 175);
tashworth	16:8bb212df81b7	523	wait(5);
tashworth	20:55dcff40c5d9	524	setServoPulse(0, Arm_Table[PU_TOOL_1].base_rotate - 1);
tashworth	16:8bb212df81b7	525	wait(1);
tashworth	16:8bb212df81b7	526	setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm - 5);
tashworth	16:8bb212df81b7	527	wait(1);
tashworth	28:2bb6b0fe39d0	528	setServoPulse(4, 175);
tashworth	28:2bb6b0fe39d0	529	wait(2);
tashworth	16:8bb212df81b7	530	setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm - 6);
tashworth	16:8bb212df81b7	531	wait(.5);
tashworth	28:2bb6b0fe39d0	532	setServoPulse(5, 100);
tashworth	16:8bb212df81b7	533	wait(.5);
tashworth	16:8bb212df81b7	534	setServoPulse(5, 00);
tashworth	17:a5bb85ee205d	535	setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm - 10);
tashworth	16:8bb212df81b7	536	wait(1);
tashworth	28:2bb6b0fe39d0	537	setServoPulse(5, 120);
tashworth	16:8bb212df81b7	538	wait(1);
tashworth	16:8bb212df81b7	539	setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm);
tashworth	17:a5bb85ee205d	540	wait(2);
tashworth	28:2bb6b0fe39d0	541
tashworth	17:a5bb85ee205d	542	setServoPulse(2, Arm_Table[DRIVE_POSITION_TOOL].big_arm);
tashworth	17:a5bb85ee205d	543	setServoPulse(3, Arm_Table[DRIVE_POSITION_TOOL].claw_arm);
tashworth	17:a5bb85ee205d	544	setServoPulse(1, Arm_Table[DRIVE_POSITION_TOOL].base_arm);
tashworth	17:a5bb85ee205d	545	setServoPulse(0, Arm_Table[DRIVE_POSITION_TOOL].base_rotate);
tashworth	17:a5bb85ee205d	546	setServoPulse(4, Arm_Table[DRIVE_POSITION_TOOL].claw_rotate);
tashworth	17:a5bb85ee205d	547	setServoPulse(5, Arm_Table[DRIVE_POSITION_TOOL].claw_open);
tashworth	28:2bb6b0fe39d0	548
tashworth	28:2bb6b0fe39d0	549
tashworth	16:8bb212df81b7	550	state = NAVIGATE_WAVES_ROW1;
tashworth	16:8bb212df81b7	551	break;
tashworth	28:2bb6b0fe39d0	552
tashworth	16:8bb212df81b7	553	case AQUIRE_TOOL2:
tashworth	16:8bb212df81b7	554	servoPosition(PU_TOOL_2);
tashworth	17:a5bb85ee205d	555	setServoPulse(4, 175);
tashworth	16:8bb212df81b7	556	wait(5);
tashworth	20:55dcff40c5d9	557	setServoPulse(0, Arm_Table[PU_TOOL_2].base_rotate - 1);
tashworth	16:8bb212df81b7	558	wait(1);
tashworth	17:a5bb85ee205d	559	setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 2);
tashworth	17:a5bb85ee205d	560	wait(1);
tashworth	17:a5bb85ee205d	561	setServoPulse(3, Arm_Table[PU_TOOL_2].claw_arm - 2);
tashworth	16:8bb212df81b7	562	wait(1);
tashworth	28:2bb6b0fe39d0	563	setServoPulse(4, 175);
tashworth	28:2bb6b0fe39d0	564	wait(2);
tashworth	16:8bb212df81b7	565	setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 6);
tashworth	17:a5bb85ee205d	566	wait(2);
tashworth	28:2bb6b0fe39d0	567	setServoPulse(5, 100);
tashworth	17:a5bb85ee205d	568	wait(2);
tashworth	16:8bb212df81b7	569	setServoPulse(5, 00);
tashworth	17:a5bb85ee205d	570	setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 10);
tashworth	17:a5bb85ee205d	571	wait(2);
tashworth	28:2bb6b0fe39d0	572	setServoPulse(5, 120);
tashworth	17:a5bb85ee205d	573	wait(2);
tashworth	16:8bb212df81b7	574	setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm);
tashworth	28:2bb6b0fe39d0	575
tashworth	17:a5bb85ee205d	576	wait(2);
tashworth	28:2bb6b0fe39d0	577
tashworth	17:a5bb85ee205d	578	setServoPulse(2, Arm_Table[DRIVE_POSITION_TOOL].big_arm);
tashworth	17:a5bb85ee205d	579	setServoPulse(3, Arm_Table[DRIVE_POSITION_TOOL].claw_arm);
tashworth	17:a5bb85ee205d	580	setServoPulse(1, Arm_Table[DRIVE_POSITION_TOOL].base_arm);
tashworth	17:a5bb85ee205d	581	setServoPulse(0, Arm_Table[DRIVE_POSITION_TOOL].base_rotate);
tashworth	17:a5bb85ee205d	582	setServoPulse(4, Arm_Table[DRIVE_POSITION_TOOL].claw_rotate);
tashworth	17:a5bb85ee205d	583	setServoPulse(5, Arm_Table[DRIVE_POSITION_TOOL].claw_open);
tashworth	28:2bb6b0fe39d0	584
tashworth	28:2bb6b0fe39d0	585
tashworth	16:8bb212df81b7	586	state = NAVIGATE_WAVES_ROW1;
tashworth	16:8bb212df81b7	587	break;
tashworth	28:2bb6b0fe39d0	588
tashworth	16:8bb212df81b7	589	case AQUIRE_TOOL3:
tashworth	17:a5bb85ee205d	590	/*
tashworth	17:a5bb85ee205d	591	while( shape_alignY_done == 0) {
tashworth	17:a5bb85ee205d	592	wait(1);
tashworth	28:2bb6b0fe39d0	593
tashworth	17:a5bb85ee205d	594	servoPosition(PU_TOOL_3);
tashworth	17:a5bb85ee205d	595	shape_detected = shapeDetection();
tashworth	17:a5bb85ee205d	596	wait(2);
tashworth	28:2bb6b0fe39d0	597
tashworth	17:a5bb85ee205d	598	pc.printf("Y - Adjust to center tool\n\r");
tashworth	28:2bb6b0fe39d0	599
tashworth	17:a5bb85ee205d	600	if(get_com_y() < 50) {
tashworth	17:a5bb85ee205d	601	wait(1);
tashworth	17:a5bb85ee205d	602	slightMove(FORWARD,25);
tashworth	17:a5bb85ee205d	603	current-=(abs(leftEncoder.getPulses()11.12/PPR) + abs(rightEncoder.getPulses()11.12/PPR))/2;
tashworth	28:2bb6b0fe39d0	604
tashworth	17:a5bb85ee205d	605	} else if(get_com_y() > 70) {
tashworth	17:a5bb85ee205d	606	wait(1);
tashworth	17:a5bb85ee205d	607	slightMove(BACKWARD,25);
tashworth	17:a5bb85ee205d	608	current+=(abs(leftEncoder.getPulses()11.12/PPR) + abs(rightEncoder.getPulses()11.12/PPR))/2;
tashworth	28:2bb6b0fe39d0	609
tashworth	17:a5bb85ee205d	610	} else {
tashworth	17:a5bb85ee205d	611	shape_alignY_done = 1;
tashworth	17:a5bb85ee205d	612	}
tashworth	17:a5bb85ee205d	613	}
tashworth	28:2bb6b0fe39d0	614
tashworth	17:a5bb85ee205d	615	*/
tashworth	20:55dcff40c5d9	616	Xadjust(TOOL_3);
tashworth	28:2bb6b0fe39d0	617
tashworth	18:a0ea7ecaf4fe	618	setServoPulse(4, 175);
tashworth	16:8bb212df81b7	619	wait(5);
tashworth	22:79c5871543b5	620	setServoPulse(0, Arm_Table[PU_TOOL_3].base_rotate - 1);
tashworth	16:8bb212df81b7	621	wait(1);
jjcarr2	32:4a42f61f64a6	622	setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm - 2);
tashworth	17:a5bb85ee205d	623	wait(1);
jjcarr2	32:4a42f61f64a6	624	setServoPulse(3, Arm_Table[PU_TOOL_3].claw_arm - 2);
tashworth	16:8bb212df81b7	625	wait(1);
tashworth	28:2bb6b0fe39d0	626	setServoPulse(4, 175);
tashworth	28:2bb6b0fe39d0	627	wait(2);
tashworth	22:79c5871543b5	628	setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm - 9);
tashworth	16:8bb212df81b7	629	wait(.5);
tashworth	28:2bb6b0fe39d0	630	setServoPulse(5, 100);
tashworth	16:8bb212df81b7	631	wait(.5);
tashworth	16:8bb212df81b7	632	setServoPulse(5, 00);
tashworth	17:a5bb85ee205d	633	setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm - 10);
tashworth	16:8bb212df81b7	634	wait(1);
tashworth	28:2bb6b0fe39d0	635	setServoPulse(5, 120);
tashworth	16:8bb212df81b7	636	wait(1);
tashworth	16:8bb212df81b7	637	setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm);
tashworth	28:2bb6b0fe39d0	638
tashworth	17:a5bb85ee205d	639	wait(2);
tashworth	28:2bb6b0fe39d0	640
tashworth	17:a5bb85ee205d	641	setServoPulse(2, Arm_Table[DRIVE_POSITION_TOOL].big_arm);
tashworth	17:a5bb85ee205d	642	setServoPulse(3, Arm_Table[DRIVE_POSITION_TOOL].claw_arm);
tashworth	17:a5bb85ee205d	643	setServoPulse(1, Arm_Table[DRIVE_POSITION_TOOL].base_arm);
tashworth	17:a5bb85ee205d	644	setServoPulse(0, Arm_Table[DRIVE_POSITION_TOOL].base_rotate);
tashworth	17:a5bb85ee205d	645	setServoPulse(4, Arm_Table[DRIVE_POSITION_TOOL].claw_rotate);
tashworth	17:a5bb85ee205d	646	setServoPulse(5, Arm_Table[DRIVE_POSITION_TOOL].claw_open);
tashworth	28:2bb6b0fe39d0	647
tashworth	16:8bb212df81b7	648	state = NAVIGATE_WAVES_ROW1;
tashworth	16:8bb212df81b7	649	break;
tashworth	28:2bb6b0fe39d0	650
tashworth	28:2bb6b0fe39d0	651
tashworth	16:8bb212df81b7	652	/**************************************************
tashworth	16:8bb212df81b7	653	* STAGE 4
tashworth	16:8bb212df81b7	654	*
tashworth	16:8bb212df81b7	655	* - Navigate through the ocean
tashworth	16:8bb212df81b7	656	*
tashworth	16:8bb212df81b7	657	**************************************************/
tashworth	28:2bb6b0fe39d0	658
tashworth	16:8bb212df81b7	659	case NAVIGATE_WAVES_ROW1:
tashworth	17:a5bb85ee205d	660	from_tools_section(location,current);
tashworth	28:2bb6b0fe39d0	661
tashworth	17:a5bb85ee205d	662	mid_section(location, current, direction);
tashworth	28:2bb6b0fe39d0	663
tashworth	16:8bb212df81b7	664	state = NAVIGATE_WAVES_ROW2;
tashworth	16:8bb212df81b7	665	break;
tashworth	28:2bb6b0fe39d0	666
tashworth	16:8bb212df81b7	667	case NAVIGATE_WAVES_ROW2:
tashworth	17:a5bb85ee205d	668	mid_section2(location, current, direction);
tashworth	16:8bb212df81b7	669	state = NAVIGATE_WAVES_ROW3;
tashworth	16:8bb212df81b7	670	break;
tashworth	28:2bb6b0fe39d0	671
tashworth	16:8bb212df81b7	672	case NAVIGATE_WAVES_ROW3:
Fairy_Paolina	33:a41981e18a7d	673	tool_needed = 2;
Fairy_Paolina	23:f8e806d1ffcc	674	if(tool_needed == 1) {
tashworth	17:a5bb85ee205d	675	rig_section(location, current, direction, 1);
tashworth	22:79c5871543b5	676	//state = NAVIGATE_TO_SQUARE_RIG;
tashworth	22:79c5871543b5	677	state = RETURN_TO_START;
Fairy_Paolina	23:f8e806d1ffcc	678	} else if(tool_needed == 2) {
tashworth	17:a5bb85ee205d	679	rig_section(location, current, direction, 2);
tashworth	22:79c5871543b5	680	//state = NAVIGATE_TO_TRIANGLE_RIG;
tashworth	22:79c5871543b5	681	state = RETURN_TO_START;
tashworth	16:8bb212df81b7	682	} else {
tashworth	17:a5bb85ee205d	683	rig_section(location, current, direction, 3);
tashworth	22:79c5871543b5	684	//state = NAVIGATE_TO_CIRCLE_RIG;
tashworth	22:79c5871543b5	685	state = RETURN_TO_START;
tashworth	28:2bb6b0fe39d0	686
tashworth	16:8bb212df81b7	687	}
tashworth	16:8bb212df81b7	688	break;
tashworth	28:2bb6b0fe39d0	689
tashworth	16:8bb212df81b7	690	/**************************************************
tashworth	16:8bb212df81b7	691	* STAGE 5
tashworth	16:8bb212df81b7	692	*
tashworth	16:8bb212df81b7	693	* - Travel to appropriate rig
tashworth	16:8bb212df81b7	694	*
tashworth	16:8bb212df81b7	695	**************************************************/
tashworth	16:8bb212df81b7	696	case NAVIGATE_TO_SQUARE_RIG:
tashworth	16:8bb212df81b7	697	//NAVIGATION CODE HERE
tashworth	16:8bb212df81b7	698	state = RIG_ALIGN;
tashworth	16:8bb212df81b7	699	break;
tashworth	16:8bb212df81b7	700	case NAVIGATE_TO_TRIANGLE_RIG:
tashworth	16:8bb212df81b7	701	//NAVIGATION CODE HERE
tashworth	16:8bb212df81b7	702	state = RIG_ALIGN;
tashworth	16:8bb212df81b7	703	break;
tashworth	16:8bb212df81b7	704	case NAVIGATE_TO_CIRCLE_RIG:
tashworth	16:8bb212df81b7	705	//NAVIGATION CODE HERE
tashworth	16:8bb212df81b7	706	state = RIG_ALIGN;
tashworth	16:8bb212df81b7	707	break;
tashworth	28:2bb6b0fe39d0	708
tashworth	16:8bb212df81b7	709	/**************************************************
tashworth	16:8bb212df81b7	710	* STAGE 6
tashworth	16:8bb212df81b7	711	*
tashworth	16:8bb212df81b7	712	* - Align with appropriate rig
tashworth	16:8bb212df81b7	713	*
tashworth	16:8bb212df81b7	714	**************************************************/
tashworth	16:8bb212df81b7	715	case RIG_ALIGN:
tashworth	28:2bb6b0fe39d0	716
tashworth	16:8bb212df81b7	717	//*********************//
tashworth	16:8bb212df81b7	718	//***** TODO ******//
tashworth	16:8bb212df81b7	719	//*********************//
tashworth	16:8bb212df81b7	720	// CODE TO ALIGN ROBOT WITH RIG
tashworth	28:2bb6b0fe39d0	721
tashworth	16:8bb212df81b7	722	servoPosition(ORIENT_TOOL);
tashworth	16:8bb212df81b7	723	wait(1); //wait for servos to settle
tashworth	16:8bb212df81b7	724	state = INSERT_TOOL;
tashworth	16:8bb212df81b7	725	break;
tashworth	28:2bb6b0fe39d0	726
tashworth	16:8bb212df81b7	727	/**************************************************
tashworth	16:8bb212df81b7	728	* STAGE 7
tashworth	16:8bb212df81b7	729	*
tashworth	16:8bb212df81b7	730	* - Insert Tool
tashworth	16:8bb212df81b7	731	* - Extenguish fire
tashworth	16:8bb212df81b7	732	* - win contest
tashworth	16:8bb212df81b7	733	*
tashworth	16:8bb212df81b7	734	**************************************************/
tashworth	28:2bb6b0fe39d0	735
tashworth	16:8bb212df81b7	736	case INSERT_TOOL:
tashworth	16:8bb212df81b7	737	//*********************//
tashworth	16:8bb212df81b7	738	//***** TODO ******//
tashworth	16:8bb212df81b7	739	//*********************//
tashworth	16:8bb212df81b7	740	// CODE TO INSERT TOOL
tashworth	16:8bb212df81b7	741	break;
tashworth	28:2bb6b0fe39d0	742
tashworth	16:8bb212df81b7	743	/**************************************************
tashworth	16:8bb212df81b7	744	* STAGE 8
tashworth	16:8bb212df81b7	745	*
tashworth	22:79c5871543b5	746	* - Return to start zone
tashworth	22:79c5871543b5	747	*
tashworth	22:79c5871543b5	748	**************************************************/
tashworth	22:79c5871543b5	749	case RETURN_TO_START:
tashworth	28:2bb6b0fe39d0	750	wait(3);
tashworth	22:79c5871543b5	751	rig_section_return(location, current, direction);
tashworth	22:79c5871543b5	752	mid_section2_return(location, current, direction);
tashworth	22:79c5871543b5	753	mid_section_return(location, current, direction);
tashworth	22:79c5871543b5	754	tools_section_return(location,current);
tashworth	22:79c5871543b5	755	while(1);
tashworth	22:79c5871543b5	756	state = END;
tashworth	22:79c5871543b5	757	break;
tashworth	28:2bb6b0fe39d0	758
tashworth	22:79c5871543b5	759	/**************************************************
tashworth	22:79c5871543b5	760	* STAGE 9
tashworth	22:79c5871543b5	761	*
tashworth	16:8bb212df81b7	762	* - END COMPETITION
tashworth	16:8bb212df81b7	763	*
tashworth	16:8bb212df81b7	764	**************************************************/
tashworth	16:8bb212df81b7	765	case END:
tashworth	16:8bb212df81b7	766	servoPosition(STORE_POSITION);
tashworth	16:8bb212df81b7	767	myled1 = 1;
tashworth	16:8bb212df81b7	768	wait(.2);
tashworth	16:8bb212df81b7	769	myled2 = 1;
tashworth	16:8bb212df81b7	770	wait(.2);
tashworth	16:8bb212df81b7	771	myled3 = 1;
tashworth	16:8bb212df81b7	772	wait(.2);
tashworth	16:8bb212df81b7	773	myled4 = 1;
tashworth	16:8bb212df81b7	774	wait(.2);
tashworth	16:8bb212df81b7	775	break;
tashworth	16:8bb212df81b7	776	default:
tashworth	28:2bb6b0fe39d0	777
tashworth	16:8bb212df81b7	778	break;
tashworth	16:8bb212df81b7	779	}
tashworth	16:8bb212df81b7	780	} // End while loop
tashworth	28:2bb6b0fe39d0	781
tashworth	16:8bb212df81b7	782	} // End if for start button
tashworth	28:2bb6b0fe39d0	783
tashworth	28:2bb6b0fe39d0	784
tashworth	16:8bb212df81b7	785	} // main loop
tashworth	28:2bb6b0fe39d0	786
tashworth	28:2bb6b0fe39d0	787
tashworth	28:2bb6b0fe39d0	788
tashworth	14:784acd735b8c	789	/************
tashworth	28:2bb6b0fe39d0	790
tashworth	14:784acd735b8c	791	Servo Functions
tashworth	28:2bb6b0fe39d0	792
tashworth	14:784acd735b8c	793	**************/
tashworth	28:2bb6b0fe39d0	794
tashworth	14:784acd735b8c	795	void setServoPulse(uint8_t n, int angle)
tashworth	14:784acd735b8c	796	{
tashworth	14:784acd735b8c	797	int pulse;
tashworth	14:784acd735b8c	798	pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE - MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE);
tashworth	14:784acd735b8c	799	float pulselength = 20000; // 20,000 us per second
tashworth	14:784acd735b8c	800	int i = currentPosition[n];
tashworth	20:55dcff40c5d9	801	//pc.printf("ServoNumber: %d Begining Pulse: %d\n\r",n,currentPosition[n]);
tashworth	14:784acd735b8c	802	int pulse2;
tashworth	14:784acd735b8c	803	if(currentPosition[n] < pulse) {
tashworth	14:784acd735b8c	804	for(i; i < pulse; i++) {
tashworth	14:784acd735b8c	805	pulse2 = 4094 * i / pulselength;
tashworth	14:784acd735b8c	806	pwm.setPWM(n, 0, pulse2);
tashworth	14:784acd735b8c	807	wait_ms(3);
tashworth	14:784acd735b8c	808	}
tashworth	14:784acd735b8c	809	} else if (currentPosition[n] > pulse) {
tashworth	14:784acd735b8c	810	for(i; i > pulse; i--) {
tashworth	14:784acd735b8c	811	pulse2 = 4094 * i / pulselength;
tashworth	14:784acd735b8c	812	pwm.setPWM(n, 0, pulse2);
tashworth	14:784acd735b8c	813	wait_ms(3);
tashworth	13:529323807361	814	}
tashworth	14:784acd735b8c	815	}
tashworth	14:784acd735b8c	816	currentPosition[n] = i;
tashworth	20:55dcff40c5d9	817	//pc.printf("END: pulse: %d, angle: %d\n\r", i, angle);
tashworth	14:784acd735b8c	818	}
tashworth	28:2bb6b0fe39d0	819
tashworth	14:784acd735b8c	820	void initServoDriver(void)
tashworth	14:784acd735b8c	821	{
tashworth	14:784acd735b8c	822	pwm.begin();
tashworth	14:784acd735b8c	823	//pwm.setPWMFreq(100); //This dosen't work well because of uncertain clock speed. Use setPrescale().
tashworth	14:784acd735b8c	824	pwm.setPrescale(140); //This value is decided for 20ms interval.
tashworth	14:784acd735b8c	825	pwm.setI2Cfreq(400000); //400kHz
tashworth	28:2bb6b0fe39d0	826
tashworth	14:784acd735b8c	827	}
tashworth	28:2bb6b0fe39d0	828
tashworth	14:784acd735b8c	829	void servoBegin(void)
tashworth	14:784acd735b8c	830	{
tashworth	14:784acd735b8c	831	pc.printf("Setting Initial Servo Position\n\r");
tashworth	16:8bb212df81b7	832	setServoPulseNo_delay(3, Arm_Table[STORE_POSITION].claw_arm);
tashworth	16:8bb212df81b7	833	setServoPulseNo_delay(2, Arm_Table[STORE_POSITION].big_arm);
tashworth	16:8bb212df81b7	834	wait(2);
tashworth	16:8bb212df81b7	835	setServoPulseNo_delay(1, Arm_Table[STORE_POSITION].base_arm);
tashworth	16:8bb212df81b7	836	setServoPulseNo_delay(0, Arm_Table[STORE_POSITION].base_rotate);
tashworth	16:8bb212df81b7	837	setServoPulseNo_delay(4, Arm_Table[STORE_POSITION].claw_rotate);
tashworth	16:8bb212df81b7	838	setServoPulseNo_delay(5, Arm_Table[STORE_POSITION].claw_open);
tashworth	14:784acd735b8c	839	}
tashworth	28:2bb6b0fe39d0	840
tashworth	14:784acd735b8c	841	void setServoPulseNo_delay(uint8_t n, int angle)
tashworth	14:784acd735b8c	842	{
tashworth	14:784acd735b8c	843	int pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE - MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE);
tashworth	14:784acd735b8c	844	float pulselength = 20000; // 20,000 us per second
tashworth	14:784acd735b8c	845	currentPosition[n] = pulse;
tashworth	14:784acd735b8c	846	//pc.printf("ServoNumber: %d Pulsewidth: %d Angle: %d \n\r",n,pulse, angle);
tashworth	14:784acd735b8c	847	pulse = 4094 * pulse / pulselength;
tashworth	14:784acd735b8c	848	pwm.setPWM(n, 0, pulse);
tashworth	28:2bb6b0fe39d0	849
tashworth	14:784acd735b8c	850	}
tashworth	17:a5bb85ee205d	851	void setServoPulse2(uint8_t n, float angle)
tashworth	17:a5bb85ee205d	852	{
tashworth	17:a5bb85ee205d	853	float pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE - MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE);
tashworth	17:a5bb85ee205d	854	float pulselength = 20000; // 10,000 us per second
tashworth	17:a5bb85ee205d	855	pulse = 4094 * pulse / pulselength;
tashworth	17:a5bb85ee205d	856	pwm.setPWM(n, 0, pulse);
tashworth	17:a5bb85ee205d	857	}
tashworth	28:2bb6b0fe39d0	858
tashworth	28:2bb6b0fe39d0	859
tashworth	28:2bb6b0fe39d0	860
tashworth	14:784acd735b8c	861	void servoPosition(int set)
tashworth	14:784acd735b8c	862	{
tashworth	14:784acd735b8c	863	//moves to current position
tashworth	14:784acd735b8c	864	setServoPulse(3, Arm_Table[set].claw_arm);
tashworth	14:784acd735b8c	865	setServoPulse(2, Arm_Table[set].big_arm);
tashworth	14:784acd735b8c	866	setServoPulse(1, Arm_Table[set].base_arm);
tashworth	14:784acd735b8c	867	setServoPulse(0, Arm_Table[set].base_rotate);
tashworth	14:784acd735b8c	868	setServoPulse(4, Arm_Table[set].claw_rotate);
tashworth	14:784acd735b8c	869	setServoPulse(5, Arm_Table[set].claw_open);
tashworth	14:784acd735b8c	870	}
Fairy_Paolina	27:5540aa3c828c	871
tashworth	14:784acd735b8c	872	int fire_checker(int rig)
tashworth	14:784acd735b8c	873	{
tashworth	14:784acd735b8c	874	switch (rig) {
tashworth	28:2bb6b0fe39d0	875
tashworth	14:784acd735b8c	876	case 1:
tashworth	14:784acd735b8c	877	for (int i = 0; i<10; i++) {
tashworth	14:784acd735b8c	878	distLaser = getDistance();
tashworth	14:784acd735b8c	879	pc.printf("L DISTANCE: %d \n\r", distLaser);
tashworth	14:784acd735b8c	880	if ((distLaser < OILRIG1_MAX)
tashworth	14:784acd735b8c	881	&& (distLaser > OILRIG1_MIN)) {
tashworth	14:784acd735b8c	882	fire_detected++;
tashworth	8:77a57909aa15	883	} else {
tashworth	14:784acd735b8c	884	fire_not_detected++;
tashworth	8:77a57909aa15	885	}
tashworth	14:784acd735b8c	886	}
tashworth	14:784acd735b8c	887	break;
tashworth	14:784acd735b8c	888	case 2:
tashworth	14:784acd735b8c	889	for (int i = 0; i<10; i++) {
tashworth	14:784acd735b8c	890	distLaser = getDistance();
tashworth	14:784acd735b8c	891	pc.printf("L DISTANCE: %d \n\r", distLaser);
tashworth	14:784acd735b8c	892	if ((distLaser < OILRIG2_MAX)
tashworth	14:784acd735b8c	893	&& (distLaser > OILRIG2_MIN)) {
tashworth	14:784acd735b8c	894	fire_detected++;
tashworth	13:529323807361	895	} else {
tashworth	14:784acd735b8c	896	fire_not_detected++;
tashworth	13:529323807361	897	}
tashworth	13:529323807361	898	}
tashworth	14:784acd735b8c	899	break;
tashworth	28:2bb6b0fe39d0	900
tashworth	14:784acd735b8c	901	}
tashworth	28:2bb6b0fe39d0	902
tashworth	14:784acd735b8c	903	if (fire_detected > 0) {
tashworth	14:784acd735b8c	904	return 1;
tashworth	14:784acd735b8c	905	} else {
tashworth	14:784acd735b8c	906	return 0;
tashworth	14:784acd735b8c	907	}
tashworth	14:784acd735b8c	908	}
tashworth	28:2bb6b0fe39d0	909
tashworth	14:784acd735b8c	910	void errFunction(void)
tashworth	14:784acd735b8c	911	{
tashworth	20:55dcff40c5d9	912	pc.printf("\n\nERROR: %d", motors.getError() );
tashworth	28:2bb6b0fe39d0	913
tashworth	14:784acd735b8c	914	}
tashworth	28:2bb6b0fe39d0	915
tashworth	28:2bb6b0fe39d0	916
tashworth	14:784acd735b8c	917	/* MODIFIED WALL_FOLLOW FOR NAVIGATION */
tashworth	28:2bb6b0fe39d0	918
tashworth	14:784acd735b8c	919	void wall_follow2(int side, int direction, int section, float location, int rig)
tashworth	14:784acd735b8c	920	{
Fairy_Paolina	30:db07aea6d119	921	int dir=1, limit=78, lowlim=4;
Fairy_Paolina	27:5540aa3c828c	922	float set=28, loc=0, Rigloc=0, location_cal=0;
tashworth	28:2bb6b0fe39d0	923
tashworth	14:784acd735b8c	924	if(rig == 1) Rigloc= 16;
tashworth	14:784acd735b8c	925	else if(rig == 2) Rigloc= 45;
tashworth	14:784acd735b8c	926	else if(rig== 3) Rigloc = 70;
tashworth	28:2bb6b0fe39d0	927
tashworth	14:784acd735b8c	928	pid1.reset();
tashworth	28:2bb6b0fe39d0	929
tashworth	14:784acd735b8c	930	if(section == TOOLS) {
Fairy_Paolina	23:f8e806d1ffcc	931	limit = 100;
Fairy_Paolina	23:f8e806d1ffcc	932	lowlim=10;
tashworth	28:2bb6b0fe39d0	933	} else if(section == RIGS) set = 18;
Fairy_Paolina	30:db07aea6d119	934	else if(section == MID2) limit =80;
tashworth	28:2bb6b0fe39d0	935
tashworth	22:79c5871543b5	936	if(direction == BACKWARD) {
tashworth	22:79c5871543b5	937	dir=-1;
tashworth	22:79c5871543b5	938	limit = 100;
tashworth	22:79c5871543b5	939	} else if(direction == FORWARD) lowlim=-20;
tashworth	28:2bb6b0fe39d0	940
tashworth	20:55dcff40c5d9	941	if(location <4) limit=80;
tashworth	28:2bb6b0fe39d0	942
tashworth	14:784acd735b8c	943	leftEncoder.reset();
tashworth	14:784acd735b8c	944	rightEncoder.reset();
tashworth	28:2bb6b0fe39d0	945
tashworth	20:55dcff40c5d9	946	//pc.printf("dirloc+location %f\r\n",dirloc + location );
tashworth	20:55dcff40c5d9	947	//pc.printf("limit %d \r\n", limit);
Fairy_Paolina	23:f8e806d1ffcc	948	if((section == RIGS \|\| section == RETURN) && side == LEFT)location_cal= -1dirloc + location;
Fairy_Paolina	23:f8e806d1ffcc	949	else location_cal= dir*loc + location;
tashworth	28:2bb6b0fe39d0	950
Fairy_Paolina	23:f8e806d1ffcc	951	while((location_cal <= limit) && (location_cal >= lowlim)) {
tashworth	28:2bb6b0fe39d0	952
tashworth	14:784acd735b8c	953	loc=(abs(leftEncoder.getPulses()11.12/PPR) + abs(rightEncoder.getPulses()11.12/PPR))/2;
tashworth	28:2bb6b0fe39d0	954
Fairy_Paolina	23:f8e806d1ffcc	955	if((section == RIGS \|\| section == RETURN) && side == LEFT)location_cal= -1dirloc + location;
Fairy_Paolina	23:f8e806d1ffcc	956	else location_cal= dir*loc + location;
tashworth	28:2bb6b0fe39d0	957
tashworth	14:784acd735b8c	958	pid1.setInputLimits(0.0, set);
Fairy_Paolina	30:db07aea6d119	959	pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
tashworth	14:784acd735b8c	960	pid1.setSetPoint(set);
tashworth	28:2bb6b0fe39d0	961
Fairy_Paolina	30:db07aea6d119	962	if(side== LEFT) {
Fairy_Paolina	30:db07aea6d119	963	range = IRLeftFront.getIRDistance();
Fairy_Paolina	30:db07aea6d119	964	range2= IRLeftBack.getIRDistance();
tashworth	14:784acd735b8c	965	} else {
Fairy_Paolina	30:db07aea6d119	966	range = IRRightFront.getIRDistance();
Fairy_Paolina	30:db07aea6d119	967	range2= IRRightBack.getIRDistance();
tashworth	14:784acd735b8c	968	}
tashworth	28:2bb6b0fe39d0	969
Fairy_Paolina	30:db07aea6d119	970	if(section == RIGS) {// check for the rigs on opposite side
tashworth	28:2bb6b0fe39d0	971	if(side == LEFT) {
Fairy_Paolina	30:db07aea6d119	972	range = IRRightFront.getIRDistance();
Fairy_Paolina	30:db07aea6d119	973	range2= IRRightBack.getIRDistance();
Fairy_Paolina	27:5540aa3c828c	974
tashworth	28:2bb6b0fe39d0	975	} else {
Fairy_Paolina	30:db07aea6d119	976	range = IRLeftFront.getIRDistance();
Fairy_Paolina	30:db07aea6d119	977	range2= IRLeftBack.getIRDistance();
Fairy_Paolina	23:f8e806d1ffcc	978	}
tashworth	28:2bb6b0fe39d0	979
Fairy_Paolina	30:db07aea6d119	980	if(range2 < 40 \|\| range < 40 ) {
Fairy_Paolina	23:f8e806d1ffcc	981	if( abs(location_cal - Rigloc) < 10) {
tashworth	14:784acd735b8c	982	//STOP
tashworth	17:a5bb85ee205d	983	motors.stopBothMotors(127);
tashworth	14:784acd735b8c	984	break;
tashworth	12:284be46593ae	985	}
tashworth	12:284be46593ae	986	}
tashworth	12:284be46593ae	987	}
srsmitherman	31:99cf9c25b0f2	988
Fairy_Paolina	30:db07aea6d119	989	if(range > 40 && range2 > 40 && section != RIGS) {
srsmitherman	31:99cf9c25b0f2	990	pc.printf("RANGE %f \tRANGE3 %f\r\n",range,range3);
srsmitherman	31:99cf9c25b0f2	991	//STOP
srsmitherman	31:99cf9c25b0f2	992	motors.stopBothMotors(127);
srsmitherman	31:99cf9c25b0f2	993	break;
tashworth	28:2bb6b0fe39d0	994
tashworth	14:784acd735b8c	995	} else {
Fairy_Paolina	30:db07aea6d119	996	if(direction == FORWARD) pid1.setProcessValue(range);
Fairy_Paolina	30:db07aea6d119	997	else pid1.setProcessValue(range2);
Fairy_Paolina	30:db07aea6d119	998
srsmitherman	31:99cf9c25b0f2	999	if(abs(range - range2) < 10) { // does it see a wavegap?
Fairy_Paolina	30:db07aea6d119	1000	pid_return = pid1.compute();
Fairy_Paolina	30:db07aea6d119	1001	//pc.printf("Range: %f\n PID: %f\r\n", range, pid_return);
tashworth	28:2bb6b0fe39d0	1002
Fairy_Paolina	30:db07aea6d119	1003	if(pid_return > 0) {
Fairy_Paolina	30:db07aea6d119	1004	if(side) {
Fairy_Paolina	30:db07aea6d119	1005	motors.setMotor0Speed(dirMAX_SPEED - dirpid_return);//right
Fairy_Paolina	30:db07aea6d119	1006	motors.setMotor1Speed(dir*MAX_SPEED);//left
Fairy_Paolina	30:db07aea6d119	1007	} else {
Fairy_Paolina	30:db07aea6d119	1008	motors.setMotor1Speed(dirMAX_SPEED - dirpid_return);//left
Fairy_Paolina	30:db07aea6d119	1009	motors.setMotor0Speed(dir*MAX_SPEED);//right
Fairy_Paolina	30:db07aea6d119	1010	}
Fairy_Paolina	30:db07aea6d119	1011	} else if(pid_return < 0) {
Fairy_Paolina	30:db07aea6d119	1012	if(side) {
Fairy_Paolina	30:db07aea6d119	1013	motors.setMotor0Speed(dir*MAX_SPEED);//right
Fairy_Paolina	30:db07aea6d119	1014	motors.setMotor1Speed(dirMAX_SPEED + dirpid_return);//left
Fairy_Paolina	30:db07aea6d119	1015	} else {
Fairy_Paolina	30:db07aea6d119	1016	motors.setMotor1Speed(dir*MAX_SPEED);//left
Fairy_Paolina	30:db07aea6d119	1017	motors.setMotor0Speed(dirMAX_SPEED + dirpid_return);//right
Fairy_Paolina	30:db07aea6d119	1018	}
tashworth	14:784acd735b8c	1019	} else {
Fairy_Paolina	30:db07aea6d119	1020	motors.setMotor0Speed(dir*MAX_SPEED);
Fairy_Paolina	30:db07aea6d119	1021	motors.setMotor1Speed(dir*MAX_SPEED);
tashworth	12:284be46593ae	1022	}
tashworth	14:784acd735b8c	1023	}
tashworth	14:784acd735b8c	1024	}
tashworth	14:784acd735b8c	1025	}
tashworth	28:2bb6b0fe39d0	1026
tashworth	14:784acd735b8c	1027	//STOP
tashworth	17:a5bb85ee205d	1028	motors.stopBothMotors(127);
tashworth	14:784acd735b8c	1029	}
tashworth	28:2bb6b0fe39d0	1030
tashworth	28:2bb6b0fe39d0	1031
tashworth	14:784acd735b8c	1032	void alignWithWall(int section)
tashworth	14:784acd735b8c	1033	{
tashworth	14:784acd735b8c	1034	float usValue = 0;
tashworth	28:2bb6b0fe39d0	1035
tashworth	14:784acd735b8c	1036	if(section == TOOLS) {
tashworth	20:55dcff40c5d9	1037	//pc.printf("tools section align\r\n");
tashworth	14:784acd735b8c	1038	// turn at an angle
tashworth	14:784acd735b8c	1039	leftEncoder.reset();
tashworth	14:784acd735b8c	1040	rightEncoder.reset();
tashworth	14:784acd735b8c	1041	motors.setMotor0Speed(-1.2*MAX_SPEED); //right
tashworth	14:784acd735b8c	1042	motors.setMotor1Speed(0.4*MAX_SPEED); //left
tashworth	14:784acd735b8c	1043	while(rightEncoder.getPulses()>-1000);
tashworth	14:784acd735b8c	1044	motors.stopBothMotors(0);
tashworth	28:2bb6b0fe39d0	1045
tashworth	14:784acd735b8c	1046	//go backwards toward wall
tashworth	14:784acd735b8c	1047	leftEncoder.reset();
tashworth	14:784acd735b8c	1048	rightEncoder.reset();
tashworth	14:784acd735b8c	1049	motors.setMotor0Speed(-0.25*127); //right
tashworth	14:784acd735b8c	1050	motors.setMotor1Speed(-0.25*127); //left
tashworth	14:784acd735b8c	1051	while(abs(leftEncoder.getPulses()) < 300 \|\| abs(rightEncoder.getPulses()) < 300);
tashworth	14:784acd735b8c	1052	motors.stopBothMotors(0);
tashworth	28:2bb6b0fe39d0	1053
tashworth	14:784acd735b8c	1054	// turn left towards wall
tashworth	14:784acd735b8c	1055	leftEncoder.reset();
tashworth	14:784acd735b8c	1056	rightEncoder.reset();
tashworth	14:784acd735b8c	1057	motors.setMotor0Speed(MAX_SPEED); //right
tashworth	14:784acd735b8c	1058	motors.setMotor1Speed(-MAX_SPEED); //left
tashworth	14:784acd735b8c	1059	while(rightEncoder.getPulses() < 20 \|\| abs(leftEncoder.getPulses()) < 20);
tashworth	28:2bb6b0fe39d0	1060
tashworth	20:55dcff40c5d9	1061	motors.stopBothMotors(127);
tashworth	20:55dcff40c5d9	1062	wait_ms(300);
tashworth	20:55dcff40c5d9	1063	return;
tashworth	28:2bb6b0fe39d0	1064
tashworth	17:a5bb85ee205d	1065	} else if(section == RIGS) {
tashworth	28:2bb6b0fe39d0	1066
Fairy_Paolina	23:f8e806d1ffcc	1067	// turn left at an angle
tashworth	14:784acd735b8c	1068	leftEncoder.reset();
tashworth	14:784acd735b8c	1069	rightEncoder.reset();
tashworth	14:784acd735b8c	1070	motors.setMotor1Speed(-1.2*MAX_SPEED); //left
tashworth	14:784acd735b8c	1071	motors.setMotor0Speed(0.4*MAX_SPEED); //right
tashworth	17:a5bb85ee205d	1072	while(abs(leftEncoder.getPulses())<500);
tashworth	14:784acd735b8c	1073	motors.stopBothMotors(0);
Fairy_Paolina	23:f8e806d1ffcc	1074	wait_ms(200);
tashworth	28:2bb6b0fe39d0	1075
Fairy_Paolina	23:f8e806d1ffcc	1076	//go backwards away form wall
tashworth	20:55dcff40c5d9	1077	leftEncoder.reset();
tashworth	20:55dcff40c5d9	1078	rightEncoder.reset();
tashworth	20:55dcff40c5d9	1079	motors.setMotor0Speed(-0.25*127); //right
tashworth	20:55dcff40c5d9	1080	motors.setMotor1Speed(-0.25*127); //left
tashworth	20:55dcff40c5d9	1081	while(abs(leftEncoder.getPulses()) < 150 \|\| abs(rightEncoder.getPulses()) < 150);
Fairy_Paolina	23:f8e806d1ffcc	1082	motors.stopBothMotors(127);
Fairy_Paolina	33:a41981e18a7d	1083	wait(1);
tashworth	28:2bb6b0fe39d0	1084
Fairy_Paolina	33:a41981e18a7d	1085	if(IRLeftFront.getIRDistance() > 35 && IRLeftBack.getIRDistance() > 35) return;
Fairy_Paolina	33:a41981e18a7d	1086	// align with wave
Fairy_Paolina	33:a41981e18a7d	1087	while(IRLeftFront.getIRDistance() > 35) {
Fairy_Paolina	33:a41981e18a7d	1088	pc.printf("front sensor sees gap\r\n");
Fairy_Paolina	33:a41981e18a7d	1089	motors.setMotor0Speed(-0.1*127);//right
Fairy_Paolina	33:a41981e18a7d	1090	motors.setMotor1Speed(-0.1*127);//left
Fairy_Paolina	33:a41981e18a7d	1091	}
Fairy_Paolina	33:a41981e18a7d	1092	while(IRLeftBack.getIRDistance() >35 ) {
Fairy_Paolina	33:a41981e18a7d	1093	pc.printf("back sensor sees gap\r\n");
Fairy_Paolina	33:a41981e18a7d	1094	motors.setMotor0Speed(0.1*127);//right
Fairy_Paolina	33:a41981e18a7d	1095	motors.setMotor1Speed(0.1*127);//left
Fairy_Paolina	33:a41981e18a7d	1096	}
tashworth	20:55dcff40c5d9	1097	motors.stopBothMotors(127);
Fairy_Paolina	23:f8e806d1ffcc	1098
Fairy_Paolina	33:a41981e18a7d	1099	}
tashworth	14:784acd735b8c	1100	}
tashworth	28:2bb6b0fe39d0	1101
tashworth	14:784acd735b8c	1102	void rightTurn(void)
tashworth	14:784acd735b8c	1103	{
tashworth	14:784acd735b8c	1104	motors.begin();
tashworth	14:784acd735b8c	1105	leftEncoder.reset();
tashworth	14:784acd735b8c	1106	rightEncoder.reset();
tashworth	14:784acd735b8c	1107	motors.setMotor0Speed(-0.5*127);//right
tashworth	14:784acd735b8c	1108	motors.setMotor1Speed(0.5*127);//left
Fairy_Paolina	24:3369d51f6cbd	1109	while(abs(leftEncoder.getPulses())<800 \|\| abs(rightEncoder.getPulses())<800);
tashworth	17:a5bb85ee205d	1110	motors.stopBothMotors(127);
Fairy_Paolina	27:5540aa3c828c	1111	wait_ms(300);
tashworth	28:2bb6b0fe39d0	1112
Fairy_Paolina	30:db07aea6d119	1113	if(IRLeftFront.getIRDistance() > 35 && IRLeftBack.getIRDistance() > 35) return;
Fairy_Paolina	27:5540aa3c828c	1114	// align with wave
Fairy_Paolina	30:db07aea6d119	1115	while(IRLeftFront.getIRDistance() > 35) {
Fairy_Paolina	27:5540aa3c828c	1116	pc.printf("front sensor sees gap\r\n");
Fairy_Paolina	27:5540aa3c828c	1117	motors.setMotor0Speed(-0.1*127);//right
Fairy_Paolina	27:5540aa3c828c	1118	motors.setMotor1Speed(-0.1*127);//left
Fairy_Paolina	27:5540aa3c828c	1119	}
Fairy_Paolina	30:db07aea6d119	1120	while(IRLeftBack.getIRDistance() >35 ) {
tashworth	28:2bb6b0fe39d0	1121	pc.printf("back sensor sees gap\r\n");
Fairy_Paolina	27:5540aa3c828c	1122	motors.setMotor0Speed(0.1*127);//right
tashworth	28:2bb6b0fe39d0	1123	motors.setMotor1Speed(0.1*127);//left
Fairy_Paolina	27:5540aa3c828c	1124	}
Fairy_Paolina	27:5540aa3c828c	1125	motors.stopBothMotors(127);
Fairy_Paolina	27:5540aa3c828c	1126	wait_ms(300);
tashworth	28:2bb6b0fe39d0	1127
Fairy_Paolina	30:db07aea6d119	1128	while(IRLeftFront.getIRDistance() - IRLeftBack.getIRDistance() > 0.5) {
tashworth	28:2bb6b0fe39d0	1129	pc.printf("turn left\r\n");
tashworth	28:2bb6b0fe39d0	1130	motors.setMotor0Speed(0.3*127);// right
tashworth	28:2bb6b0fe39d0	1131	motors.setMotor1Speed(-0.3*127);// left
Fairy_Paolina	27:5540aa3c828c	1132	}
Fairy_Paolina	30:db07aea6d119	1133	while(IRLeftFront.getIRDistance() - IRLeftBack.getIRDistance() < -0.5) {
tashworth	28:2bb6b0fe39d0	1134	pc.printf("turn right\r\n");
tashworth	28:2bb6b0fe39d0	1135	motors.setMotor0Speed(-0.3*127);//right
tashworth	28:2bb6b0fe39d0	1136	motors.setMotor1Speed(0.3*127);// left
Fairy_Paolina	27:5540aa3c828c	1137	}
tashworth	28:2bb6b0fe39d0	1138
Fairy_Paolina	27:5540aa3c828c	1139	motors.stopBothMotors(127);
Fairy_Paolina	27:5540aa3c828c	1140	wait_ms(300);
tashworth	28:2bb6b0fe39d0	1141
tashworth	14:784acd735b8c	1142	}
tashworth	28:2bb6b0fe39d0	1143
tashworth	14:784acd735b8c	1144	void leftTurn(void)
tashworth	14:784acd735b8c	1145	{
tashworth	14:784acd735b8c	1146	motors.begin();
tashworth	14:784acd735b8c	1147	leftEncoder.reset();
tashworth	14:784acd735b8c	1148	rightEncoder.reset();
tashworth	14:784acd735b8c	1149	motors.setMotor0Speed(0.5*127);// right
tashworth	14:784acd735b8c	1150	motors.setMotor1Speed(-0.5*127);// left
tashworth	20:55dcff40c5d9	1151	while(abs(leftEncoder.getPulses())<1075 \|\| rightEncoder.getPulses()<1075);
tashworth	17:a5bb85ee205d	1152	motors.stopBothMotors(127);
Fairy_Paolina	27:5540aa3c828c	1153	wait_ms(300);
tashworth	28:2bb6b0fe39d0	1154
Fairy_Paolina	30:db07aea6d119	1155	if(IRLeftFront.getIRDistance() > 35 && IRLeftBack.getIRDistance() > 35) return;
Fairy_Paolina	27:5540aa3c828c	1156	// align with wave
Fairy_Paolina	30:db07aea6d119	1157	while(IRLeftFront.getIRDistance() > 35) {
Fairy_Paolina	27:5540aa3c828c	1158	pc.printf("front sensor sees gap\r\n");
Fairy_Paolina	27:5540aa3c828c	1159	motors.setMotor0Speed(-0.1*127);//right
Fairy_Paolina	27:5540aa3c828c	1160	motors.setMotor1Speed(-0.1*127);//left
Fairy_Paolina	27:5540aa3c828c	1161	}
Fairy_Paolina	30:db07aea6d119	1162	while(IRLeftBack.getIRDistance() >35 ) {
tashworth	28:2bb6b0fe39d0	1163	pc.printf("back sensor sees gap\r\n");
Fairy_Paolina	27:5540aa3c828c	1164	motors.setMotor0Speed(0.1*127);//right
tashworth	28:2bb6b0fe39d0	1165	motors.setMotor1Speed(0.1*127);//left
Fairy_Paolina	27:5540aa3c828c	1166	}
Fairy_Paolina	27:5540aa3c828c	1167	motors.stopBothMotors(127);
Fairy_Paolina	27:5540aa3c828c	1168	wait_ms(300);
tashworth	28:2bb6b0fe39d0	1169
Fairy_Paolina	30:db07aea6d119	1170	while(IRLeftFront.getIRDistance() - IRLeftBack.getIRDistance() > 0.5) {
tashworth	28:2bb6b0fe39d0	1171	pc.printf("turn left\r\n");
tashworth	28:2bb6b0fe39d0	1172	motors.setMotor0Speed(0.3*127);// right
tashworth	28:2bb6b0fe39d0	1173	motors.setMotor1Speed(-0.3*127);// left
Fairy_Paolina	27:5540aa3c828c	1174	}
Fairy_Paolina	30:db07aea6d119	1175	while(IRLeftFront.getIRDistance() - IRLeftBack.getIRDistance() < -0.5) {
tashworth	28:2bb6b0fe39d0	1176	pc.printf("turn right\r\n");
tashworth	28:2bb6b0fe39d0	1177	motors.setMotor0Speed(-0.3*127);//right
tashworth	28:2bb6b0fe39d0	1178	motors.setMotor1Speed(0.3*127);// left
Fairy_Paolina	27:5540aa3c828c	1179	}
tashworth	28:2bb6b0fe39d0	1180
Fairy_Paolina	27:5540aa3c828c	1181	motors.stopBothMotors(127);
tashworth	28:2bb6b0fe39d0	1182	wait_ms(300);
tashworth	28:2bb6b0fe39d0	1183
tashworth	14:784acd735b8c	1184	}
tashworth	28:2bb6b0fe39d0	1185
tashworth	14:784acd735b8c	1186	void slightleft(void)
tashworth	14:784acd735b8c	1187	{
Fairy_Paolina	30:db07aea6d119	1188	motors.begin();
tashworth	14:784acd735b8c	1189	leftEncoder.reset();
tashworth	14:784acd735b8c	1190	rightEncoder.reset();
tashworth	14:784acd735b8c	1191	motors.setMotor0Speed(0.5*127);// right
Fairy_Paolina	30:db07aea6d119	1192	motors.setMotor1Speed(0.5*127);// left
srsmitherman	31:99cf9c25b0f2	1193	while(abs(leftEncoder.getPulses())<50 \|\| abs(rightEncoder.getPulses())<50) {
srsmitherman	31:99cf9c25b0f2	1194
Fairy_Paolina	30:db07aea6d119	1195	}
tashworth	17:a5bb85ee205d	1196	motors.stopBothMotors(127);
tashworth	14:784acd735b8c	1197	}
tashworth	28:2bb6b0fe39d0	1198
tashworth	14:784acd735b8c	1199	void slightright(void)
tashworth	14:784acd735b8c	1200	{
tashworth	28:2bb6b0fe39d0	1201
tashworth	14:784acd735b8c	1202	leftEncoder.reset();
tashworth	14:784acd735b8c	1203	rightEncoder.reset();
tashworth	14:784acd735b8c	1204	motors.setMotor0Speed(-0.4*127);// right
tashworth	14:784acd735b8c	1205	motors.setMotor1Speed(0.4*127);// left
tashworth	20:55dcff40c5d9	1206	while(abs(leftEncoder.getPulses())<200 \|\| abs(rightEncoder.getPulses())<200);
tashworth	17:a5bb85ee205d	1207	motors.stopBothMotors(127);
tashworth	14:784acd735b8c	1208	}
tashworth	28:2bb6b0fe39d0	1209
tashworth	14:784acd735b8c	1210	void slightMove(int direction, float pulses)
tashworth	14:784acd735b8c	1211	{
tashworth	14:784acd735b8c	1212	int dir=1;
tashworth	28:2bb6b0fe39d0	1213
tashworth	14:784acd735b8c	1214	if(direction == BACKWARD) dir= -1;
tashworth	28:2bb6b0fe39d0	1215
tashworth	14:784acd735b8c	1216	leftEncoder.reset();
tashworth	14:784acd735b8c	1217	rightEncoder.reset();
tashworth	14:784acd735b8c	1218	motors.setMotor0Speed(dir0.25127); //right
jjcarr2	32:4a42f61f64a6	1219	motors.setMotor1Speed(dir0.23127); //left
tashworth	14:784acd735b8c	1220	while(abs(leftEncoder.getPulses()) < pulses \|\| abs(rightEncoder.getPulses()) < pulses);
tashworth	28:2bb6b0fe39d0	1221
tashworth	14:784acd735b8c	1222	motors.stopBothMotors(127);
tashworth	14:784acd735b8c	1223	}
tashworth	28:2bb6b0fe39d0	1224
tashworth	14:784acd735b8c	1225	void UntilWall(int dir)
tashworth	14:784acd735b8c	1226	{
tashworth	28:2bb6b0fe39d0	1227
tashworth	14:784acd735b8c	1228	if(dir == BACKWARD) dir=-1;
tashworth	28:2bb6b0fe39d0	1229
tashworth	14:784acd735b8c	1230	leftEncoder.reset();
tashworth	14:784acd735b8c	1231	rightEncoder.reset();
tashworth	14:784acd735b8c	1232	motors.setMotor0Speed(dir0.2127); //right
tashworth	14:784acd735b8c	1233	motors.setMotor1Speed(dir0.2127); //left
tashworth	28:2bb6b0fe39d0	1234
tashworth	14:784acd735b8c	1235	range = 30;
tashworth	28:2bb6b0fe39d0	1236
tashworth	14:784acd735b8c	1237	while(range > 20) {
tashworth	14:784acd735b8c	1238	rangeFinderRight.startMeas();
tashworth	14:784acd735b8c	1239	wait_ms(20);
tashworth	14:784acd735b8c	1240	rangeFinderRight.getMeas(range);
tashworth	14:784acd735b8c	1241	}
tashworth	28:2bb6b0fe39d0	1242
tashworth	17:a5bb85ee205d	1243	motors.stopBothMotors(127);
tashworth	14:784acd735b8c	1244	}
tashworth	28:2bb6b0fe39d0	1245
tashworth	14:784acd735b8c	1246	void overBump(int section)
tashworth	14:784acd735b8c	1247	{
Fairy_Paolina	27:5540aa3c828c	1248	float avg=0;
Fairy_Paolina	27:5540aa3c828c	1249	int i;
tashworth	28:2bb6b0fe39d0	1250
Fairy_Paolina	24:3369d51f6cbd	1251	// first set
Fairy_Paolina	24:3369d51f6cbd	1252	motors.setMotor1Speed(0.1*127);//left
Fairy_Paolina	24:3369d51f6cbd	1253	motors.setMotor0Speed(0.1*127);//right
Fairy_Paolina	24:3369d51f6cbd	1254	wait_ms(100);
Fairy_Paolina	24:3369d51f6cbd	1255	while(motors.GetMotor0Current()<5 \|\| motors.GetMotor1Current() <5 );
Fairy_Paolina	24:3369d51f6cbd	1256
Fairy_Paolina	24:3369d51f6cbd	1257	while(motors.GetMotor0Current()>5 \|\| motors.GetMotor1Current() >5 ) {
Fairy_Paolina	24:3369d51f6cbd	1258	pc.printf("left %d \t right %d \r\n", motors.GetMotor0Current(), motors.GetMotor1Current());
Fairy_Paolina	24:3369d51f6cbd	1259	motors.setMotor1Speed(0.3*127);//left
Fairy_Paolina	24:3369d51f6cbd	1260	motors.setMotor0Speed(0.3*127);//right
Fairy_Paolina	24:3369d51f6cbd	1261	wait_ms(220);
Fairy_Paolina	24:3369d51f6cbd	1262	motors.stopBothMotors(127);
Fairy_Paolina	24:3369d51f6cbd	1263	wait_ms(10);
Fairy_Paolina	24:3369d51f6cbd	1264	}
Fairy_Paolina	24:3369d51f6cbd	1265
Fairy_Paolina	24:3369d51f6cbd	1266	motors.setMotor1Speed(0.1*127);//left
Fairy_Paolina	24:3369d51f6cbd	1267	motors.setMotor0Speed(0.1*127);//right
Fairy_Paolina	24:3369d51f6cbd	1268	wait_ms(100);
Fairy_Paolina	24:3369d51f6cbd	1269	while(motors.GetMotor0Current()<5 \|\| motors.GetMotor1Current() <5 );
Fairy_Paolina	24:3369d51f6cbd	1270
Fairy_Paolina	24:3369d51f6cbd	1271	motors.setMotor1Speed(0.3*127);//left
Fairy_Paolina	24:3369d51f6cbd	1272	motors.setMotor0Speed(0.3*127);//right
Fairy_Paolina	24:3369d51f6cbd	1273	// second set
tashworth	14:784acd735b8c	1274	wait_ms(200);
Fairy_Paolina	24:3369d51f6cbd	1275	motors.stopBothMotors(127);
Fairy_Paolina	27:5540aa3c828c	1276	wait_ms(500);
srsmitherman	31:99cf9c25b0f2	1277
Fairy_Paolina	24:3369d51f6cbd	1278	if(section!= RIGS) {
Fairy_Paolina	27:5540aa3c828c	1279	range = 0;
tashworth	28:2bb6b0fe39d0	1280
tashworth	28:2bb6b0fe39d0	1281	do {
Fairy_Paolina	30:db07aea6d119	1282	pc.printf("front left %f \t front right %f\r\n", IRFrontL.getIRDistance(),IRFrontR.getIRDistance());
Fairy_Paolina	30:db07aea6d119	1283	if(IRFrontL.getIRDistance() < 20 && IRFrontR.getIRDistance() < 20) {
Fairy_Paolina	27:5540aa3c828c	1284	motors.stopBothMotors(127);
Fairy_Paolina	27:5540aa3c828c	1285	wait_ms(200);
Fairy_Paolina	27:5540aa3c828c	1286	break;
Fairy_Paolina	27:5540aa3c828c	1287	}
Fairy_Paolina	27:5540aa3c828c	1288
srsmitherman	31:99cf9c25b0f2	1289	motors.setMotor1Speed(0.33*127);//left
srsmitherman	31:99cf9c25b0f2	1290	motors.setMotor0Speed(0.33*127);//right
Fairy_Paolina	24:3369d51f6cbd	1291	wait_ms(220);
Fairy_Paolina	24:3369d51f6cbd	1292	motors.stopBothMotors(127);
Fairy_Paolina	27:5540aa3c828c	1293	wait_ms(500);
tashworth	28:2bb6b0fe39d0	1294
Fairy_Paolina	30:db07aea6d119	1295	} while(IRFrontL.getIRDistance() > 20 && IRFrontR.getIRDistance() > 20);
tashworth	28:2bb6b0fe39d0	1296
Fairy_Paolina	24:3369d51f6cbd	1297	motors.setMotor1Speed(0.1*127);//left
Fairy_Paolina	24:3369d51f6cbd	1298	motors.setMotor0Speed(0.1*127);//right
Fairy_Paolina	24:3369d51f6cbd	1299	wait_ms(100);
Fairy_Paolina	24:3369d51f6cbd	1300	while(motors.GetMotor0Current()<5 \|\| motors.GetMotor1Current() <5);
Fairy_Paolina	24:3369d51f6cbd	1301	motors.stopBothMotors(127);
tashworth	28:2bb6b0fe39d0	1302	wait_ms(200);
tashworth	28:2bb6b0fe39d0	1303
Fairy_Paolina	24:3369d51f6cbd	1304	} else {
tashworth	14:784acd735b8c	1305	leftEncoder.reset();
tashworth	14:784acd735b8c	1306	rightEncoder.reset();
Fairy_Paolina	24:3369d51f6cbd	1307
tashworth	28:2bb6b0fe39d0	1308
Fairy_Paolina	24:3369d51f6cbd	1309	while(leftEncoder.getPulses()<500 \|\| rightEncoder.getPulses()<500) {
Fairy_Paolina	30:db07aea6d119	1310	pc.printf("left %d \t right %d \r\n", motors.GetMotor0Current(), motors.GetMotor1Current());
Fairy_Paolina	24:3369d51f6cbd	1311	motors.setMotor1Speed(0.1*127);//left
Fairy_Paolina	24:3369d51f6cbd	1312	motors.setMotor0Speed(0.1*127);//right
Fairy_Paolina	24:3369d51f6cbd	1313	wait_ms(220);
srsmitherman	31:99cf9c25b0f2	1314
tashworth	28:2bb6b0fe39d0	1315	if(motors.GetMotor0Current()>5 \|\| motors.GetMotor1Current() >5) {
Fairy_Paolina	30:db07aea6d119	1316	pc.printf("fast left %d \t right %d \r\n", motors.GetMotor0Current(), motors.GetMotor1Current());
Fairy_Paolina	30:db07aea6d119	1317	motors.setMotor1Speed(0.35*127);//left
Fairy_Paolina	30:db07aea6d119	1318	motors.setMotor0Speed(0.35*127);//right
Fairy_Paolina	24:3369d51f6cbd	1319	wait_ms(220);
Fairy_Paolina	24:3369d51f6cbd	1320	motors.stopBothMotors(127);
Fairy_Paolina	30:db07aea6d119	1321	wait_ms(500);
Fairy_Paolina	24:3369d51f6cbd	1322	leftEncoder.reset();
Fairy_Paolina	24:3369d51f6cbd	1323	rightEncoder.reset();
Fairy_Paolina	24:3369d51f6cbd	1324	}
Fairy_Paolina	30:db07aea6d119	1325
tashworth	12:284be46593ae	1326	}
Fairy_Paolina	30:db07aea6d119	1327	motors.stopBothMotors(127);
Fairy_Paolina	30:db07aea6d119	1328	wait_ms(200);
tashworth	14:784acd735b8c	1329	}
Fairy_Paolina	24:3369d51f6cbd	1330
Fairy_Paolina	24:3369d51f6cbd	1331
tashworth	28:2bb6b0fe39d0	1332
tashworth	28:2bb6b0fe39d0	1333
tashworth	17:a5bb85ee205d	1334	motors.stopBothMotors(127);
tashworth	14:784acd735b8c	1335	wait_ms(20);
tashworth	14:784acd735b8c	1336	motors.begin();
tashworth	28:2bb6b0fe39d0	1337
tashworth	14:784acd735b8c	1338	}
tashworth	28:2bb6b0fe39d0	1339
tashworth	28:2bb6b0fe39d0	1340
tashworth	14:784acd735b8c	1341	void to_tools_section1(float* location, float &current)
tashworth	14:784acd735b8c	1342	{
tashworth	22:79c5871543b5	1343	slightMove(FORWARD,6650);
tashworth	14:784acd735b8c	1344	current+=(abs(leftEncoder.getPulses()11.12/PPR) + abs(rightEncoder.getPulses()11.12/PPR))/2;
tashworth	28:2bb6b0fe39d0	1345
tashworth	14:784acd735b8c	1346	}
tashworth	28:2bb6b0fe39d0	1347
tashworth	14:784acd735b8c	1348	void to_tools_section2(float* location, float &current)
tashworth	14:784acd735b8c	1349	{
tashworth	20:55dcff40c5d9	1350	slightMove(FORWARD,3250);
tashworth	14:784acd735b8c	1351	current+=(abs(leftEncoder.getPulses()11.12/PPR) + abs(rightEncoder.getPulses()11.12/PPR))/2;
tashworth	28:2bb6b0fe39d0	1352
tashworth	14:784acd735b8c	1353	}
tashworth	28:2bb6b0fe39d0	1354
tashworth	14:784acd735b8c	1355	void from_tools_section(float* location, float &current)
tashworth	14:784acd735b8c	1356	{
tashworth	28:2bb6b0fe39d0	1357
Fairy_Paolina	30:db07aea6d119	1358	pc.printf("IR front %f \t back %f \r\n", IRLeftFront.getIRDistance(), IRLeftBack.getIRDistance());
tashworth	28:2bb6b0fe39d0	1359
jjcarr2	32:4a42f61f64a6	1360	if(IRLeftFront.getIRDistance() < 40 && IRLeftBack.getIRDistance() < 40) {
Fairy_Paolina	27:5540aa3c828c	1361	wall_follow2(LEFT,BACKWARD,TOOLS, current,0);
srsmitherman	31:99cf9c25b0f2	1362
Fairy_Paolina	30:db07aea6d119	1363	pc.printf("IR front %f \t back %f \r\n", IRLeftFront.getIRDistance(), IRLeftBack.getIRDistance());
tashworth	14:784acd735b8c	1364	location[0]= current - ((abs(leftEncoder.getPulses()11.12/PPR) + abs(rightEncoder.getPulses()11.12/PPR))/2);
tashworth	14:784acd735b8c	1365	current= location[0];
srsmitherman	31:99cf9c25b0f2	1366	/***********************************************
srsmitherman	31:99cf9c25b0f2	1367	ALIGN with GAP
srsmitherman	31:99cf9c25b0f2	1368	***********************************************/
srsmitherman	31:99cf9c25b0f2	1369	alignWithGap();
srsmitherman	31:99cf9c25b0f2	1370
tashworth	20:55dcff40c5d9	1371	//pc.printf("current %f \r\n",current);
tashworth	14:784acd735b8c	1372	// go backwards
tashworth	20:55dcff40c5d9	1373	//slightMove(BACKWARD,200);
tashworth	28:2bb6b0fe39d0	1374
tashworth	14:784acd735b8c	1375	wait_ms(100);
tashworth	14:784acd735b8c	1376	leftTurn();
tashworth	14:784acd735b8c	1377	overBump(TOOLS);
tashworth	14:784acd735b8c	1378	} else {
Fairy_Paolina	30:db07aea6d119	1379	pc.printf("IR front %f \t back %f \r\n", IRLeftFront.getIRDistance(), IRLeftBack.getIRDistance());
tashworth	20:55dcff40c5d9	1380	//pc.printf("else greater than 20\r\n");
srsmitherman	31:99cf9c25b0f2	1381	/***********************************************
srsmitherman	31:99cf9c25b0f2	1382	ALIGN with GAP
srsmitherman	31:99cf9c25b0f2	1383	***********************************************/
srsmitherman	31:99cf9c25b0f2	1384
srsmitherman	31:99cf9c25b0f2	1385	alignWithGap();
srsmitherman	31:99cf9c25b0f2	1386
srsmitherman	31:99cf9c25b0f2	1387
tashworth	14:784acd735b8c	1388	location[0]= current;
tashworth	14:784acd735b8c	1389	leftTurn();
tashworth	14:784acd735b8c	1390	overBump(TOOLS);
tashworth	14:784acd735b8c	1391	}
Fairy_Paolina	27:5540aa3c828c	1392	pc.printf("First Wavegap = %f\r\n",location[0]);
tashworth	28:2bb6b0fe39d0	1393
tashworth	17:a5bb85ee205d	1394	}
tashworth	28:2bb6b0fe39d0	1395
tashworth	14:784acd735b8c	1396	void mid_section(float* location, float &current, int* direction)
tashworth	28:2bb6b0fe39d0	1397	{
Fairy_Paolina	27:5540aa3c828c	1398	wait_ms(500);
tashworth	28:2bb6b0fe39d0	1399
Fairy_Paolina	30:db07aea6d119	1400	if(IRFrontL.getIRDistance() > 40 && IRFrontR.getIRDistance() > 40) {
srsmitherman	31:99cf9c25b0f2	1401
tashworth	22:79c5871543b5	1402	leftEncoder.reset();
tashworth	22:79c5871543b5	1403	rightEncoder.reset();
tashworth	22:79c5871543b5	1404	motors.setMotor0Speed(0.25*127); //right
tashworth	22:79c5871543b5	1405	motors.setMotor1Speed(0.25*127); //left
tashworth	22:79c5871543b5	1406	while(abs(leftEncoder.getPulses()) < 50 \|\| abs(rightEncoder.getPulses()) < 50);
tashworth	14:784acd735b8c	1407	direction[0]= STRAIGHT;
tashworth	14:784acd735b8c	1408	overBump(MID);
tashworth	14:784acd735b8c	1409	return;
tashworth	14:784acd735b8c	1410	}
tashworth	20:55dcff40c5d9	1411	//pc.printf("before align with wall \r\n");
tashworth	20:55dcff40c5d9	1412	//alignWithWall(MID);
tashworth	20:55dcff40c5d9	1413	//current-=4;
tashworth	20:55dcff40c5d9	1414	//wait_ms(200);
tashworth	28:2bb6b0fe39d0	1415
tashworth	20:55dcff40c5d9	1416	//if(current > 20){
tashworth	20:55dcff40c5d9	1417	//alignWithWall(MID2);
tashworth	20:55dcff40c5d9	1418	//current-=4;
tashworth	20:55dcff40c5d9	1419	//}
tashworth	20:55dcff40c5d9	1420	rightTurn();
Fairy_Paolina	24:3369d51f6cbd	1421
tashworth	20:55dcff40c5d9	1422	//pc.printf("mid section current = %f\r\n",current);
tashworth	14:784acd735b8c	1423	wall_follow2(LEFT,FORWARD,MID, current,0);
tashworth	14:784acd735b8c	1424	current+=((abs(leftEncoder.getPulses()11.12/PPR) + abs(rightEncoder.getPulses()11.12/PPR))/2);
tashworth	20:55dcff40c5d9	1425	//pc.printf("after wf2 current = %f\r\n",current);
tashworth	28:2bb6b0fe39d0	1426
tashworth	14:784acd735b8c	1427	wait_ms(500);
tashworth	28:2bb6b0fe39d0	1428
Fairy_Paolina	30:db07aea6d119	1429	if(IRLeftFront.getIRDistance() > 40 && IRLeftBack.getIRDistance() > 40) {
Fairy_Paolina	23:f8e806d1ffcc	1430	wait(1);
tashworth	14:784acd735b8c	1431	direction[0]= RIGHT;
tashworth	14:784acd735b8c	1432	location[1]= current;
tashworth	20:55dcff40c5d9	1433	wait_ms(300);
tashworth	22:79c5871543b5	1434	slightMove(FORWARD,200);
tashworth	20:55dcff40c5d9	1435	current+=((abs(leftEncoder.getPulses()11.12/PPR) + abs(rightEncoder.getPulses()11.12/PPR))/2);
tashworth	14:784acd735b8c	1436	} else {
tashworth	14:784acd735b8c	1437	direction[0]= LEFT;
tashworth	14:784acd735b8c	1438	wall_follow2(LEFT,BACKWARD,MID,current,0);
tashworth	14:784acd735b8c	1439	location[1]= current- ((abs(leftEncoder.getPulses()11.12/PPR) + abs(rightEncoder.getPulses()11.12/PPR))/2);
tashworth	14:784acd735b8c	1440	current= location[1];
tashworth	28:2bb6b0fe39d0	1441
tashworth	14:784acd735b8c	1442	if(location[1] < 18) {
tashworth	20:55dcff40c5d9	1443	slightMove(FORWARD, 75);
tashworth	14:784acd735b8c	1444	current+=((abs(leftEncoder.getPulses()11.12/PPR) + abs(rightEncoder.getPulses()11.12/PPR))/2);
tashworth	28:2bb6b0fe39d0	1445	} else slightMove(BACKWARD,75);
tashworth	28:2bb6b0fe39d0	1446
tashworth	14:784acd735b8c	1447	}
tashworth	28:2bb6b0fe39d0	1448
tashworth	20:55dcff40c5d9	1449	wait_ms(200);
tashworth	20:55dcff40c5d9	1450	//pc.printf("wavegap2 = %f\r\n",location[1]);
tashworth	20:55dcff40c5d9	1451	//left turn
srsmitherman	31:99cf9c25b0f2	1452	/***********************************************
srsmitherman	31:99cf9c25b0f2	1453	ALIGN with GAP
srsmitherman	31:99cf9c25b0f2	1454	***********************************************/
srsmitherman	31:99cf9c25b0f2	1455
srsmitherman	31:99cf9c25b0f2	1456	alignWithGap();
srsmitherman	31:99cf9c25b0f2	1457
tashworth	20:55dcff40c5d9	1458	motors.begin();
tashworth	20:55dcff40c5d9	1459	leftEncoder.reset();
tashworth	20:55dcff40c5d9	1460	rightEncoder.reset();
tashworth	20:55dcff40c5d9	1461	motors.setMotor0Speed(0.5*127);// right
tashworth	20:55dcff40c5d9	1462	motors.setMotor1Speed(-0.5*127);// left
Fairy_Paolina	23:f8e806d1ffcc	1463	while(abs(leftEncoder.getPulses())<110 \|\| rightEncoder.getPulses()<1100);
tashworth	20:55dcff40c5d9	1464	motors.stopBothMotors(127);
tashworth	28:2bb6b0fe39d0	1465
tashworth	14:784acd735b8c	1466	wait_ms(100);
tashworth	28:2bb6b0fe39d0	1467
tashworth	14:784acd735b8c	1468	overBump(MID);
tashworth	14:784acd735b8c	1469	}
tashworth	28:2bb6b0fe39d0	1470
tashworth	14:784acd735b8c	1471	void mid_section2(float* location, float &current, int* direction)
tashworth	14:784acd735b8c	1472	{
tashworth	20:55dcff40c5d9	1473	//pc.printf("mid section 2\r\n");
Fairy_Paolina	27:5540aa3c828c	1474	wait_ms(500);
tashworth	28:2bb6b0fe39d0	1475
Fairy_Paolina	30:db07aea6d119	1476	if(IRFrontL.getIRDistance() > 40 && IRFrontR.getIRDistance() > 40) {
tashworth	22:79c5871543b5	1477	leftEncoder.reset();
tashworth	22:79c5871543b5	1478	rightEncoder.reset();
tashworth	22:79c5871543b5	1479	motors.setMotor0Speed(0.25*127); //right
tashworth	22:79c5871543b5	1480	motors.setMotor1Speed(0.25*127); //left
tashworth	22:79c5871543b5	1481	while(abs(leftEncoder.getPulses()) < 50 \|\| abs(rightEncoder.getPulses()) < 50);
tashworth	14:784acd735b8c	1482	direction[1]= STRAIGHT;
tashworth	14:784acd735b8c	1483	overBump(RIGS);
tashworth	14:784acd735b8c	1484	return;
tashworth	14:784acd735b8c	1485	}
tashworth	28:2bb6b0fe39d0	1486
tashworth	20:55dcff40c5d9	1487	//alignWithWall(MID);
tashworth	20:55dcff40c5d9	1488	wait_ms(100);
tashworth	28:2bb6b0fe39d0	1489
tashworth	20:55dcff40c5d9	1490	rightTurn();
tashworth	20:55dcff40c5d9	1491	wait_ms(100);
tashworth	28:2bb6b0fe39d0	1492
tashworth	28:2bb6b0fe39d0	1493
tashworth	14:784acd735b8c	1494	wall_follow2(LEFT,FORWARD,MID, current,0);
tashworth	14:784acd735b8c	1495	current+=((abs(leftEncoder.getPulses()11.12/PPR) + abs(rightEncoder.getPulses()11.12/PPR))/2);
tashworth	28:2bb6b0fe39d0	1496
tashworth	14:784acd735b8c	1497	wait_ms(500);
tashworth	28:2bb6b0fe39d0	1498
tashworth	20:55dcff40c5d9	1499	//pc.printf("midseection 2 after wf2 %f",current);
tashworth	28:2bb6b0fe39d0	1500
Fairy_Paolina	30:db07aea6d119	1501	if(IRLeftFront.getIRDistance() > 40 && IRLeftBack.getIRDistance() > 40) {
tashworth	14:784acd735b8c	1502	direction[1]= RIGHT;
tashworth	14:784acd735b8c	1503	location[2]= current;
Fairy_Paolina	23:f8e806d1ffcc	1504	slightMove(FORWARD,150);
tashworth	17:a5bb85ee205d	1505	current+=((abs(leftEncoder.getPulses()11.12/PPR) + abs(rightEncoder.getPulses()11.12/PPR))/2);
tashworth	14:784acd735b8c	1506	} else {
tashworth	14:784acd735b8c	1507	direction[1]= LEFT;
tashworth	14:784acd735b8c	1508	wall_follow2(LEFT,BACKWARD,MID,current,0);
tashworth	14:784acd735b8c	1509	location[2]= current- ((abs(leftEncoder.getPulses()11.12/PPR) + abs(rightEncoder.getPulses()11.12/PPR))/2);
tashworth	14:784acd735b8c	1510	current=location[2];
tashworth	20:55dcff40c5d9	1511	//slightMove(BACKWARD,100);
tashworth	14:784acd735b8c	1512	}
srsmitherman	31:99cf9c25b0f2	1513	/***********************************************
srsmitherman	31:99cf9c25b0f2	1514	ALIGN with GAP
srsmitherman	31:99cf9c25b0f2	1515	***********************************************/
srsmitherman	31:99cf9c25b0f2	1516	alignWithGap();
tashworth	20:55dcff40c5d9	1517	//LEFT turn
tashworth	20:55dcff40c5d9	1518	motors.begin();
srsmitherman	31:99cf9c25b0f2	1519	motors.stopBothMotors(127);
srsmitherman	31:99cf9c25b0f2	1520	wait_ms(200);
tashworth	20:55dcff40c5d9	1521	leftEncoder.reset();
tashworth	20:55dcff40c5d9	1522	rightEncoder.reset();
tashworth	20:55dcff40c5d9	1523	motors.setMotor0Speed(0.5*127);// right
tashworth	20:55dcff40c5d9	1524	motors.setMotor1Speed(-0.5*127);// left
srsmitherman	31:99cf9c25b0f2	1525	while(abs(leftEncoder.getPulses())<1000 \|\| rightEncoder.getPulses()<1000);
tashworth	20:55dcff40c5d9	1526	motors.stopBothMotors(127);
tashworth	28:2bb6b0fe39d0	1527
tashworth	14:784acd735b8c	1528	overBump(RIGS);
tashworth	20:55dcff40c5d9	1529	//pc.printf("overbump rigs\r\n");
tashworth	14:784acd735b8c	1530	}
tashworth	28:2bb6b0fe39d0	1531
tashworth	14:784acd735b8c	1532	void rig_section(float* location, float &current, int* direction, int rig)
tashworth	14:784acd735b8c	1533	{
tashworth	14:784acd735b8c	1534	float loc;
tashworth	28:2bb6b0fe39d0	1535
tashworth	14:784acd735b8c	1536	if(rig == 1) loc= 15;
tashworth	14:784acd735b8c	1537	else if(rig == 2) loc= 45;
tashworth	14:784acd735b8c	1538	else loc = 75;
Fairy_Paolina	33:a41981e18a7d	1539	// Backwards until the gap is hit.
Fairy_Paolina	33:a41981e18a7d	1540	motors.setMotor1Speed(-0.1*127);//left
Fairy_Paolina	33:a41981e18a7d	1541	motors.setMotor0Speed(-0.1*127);//right
Fairy_Paolina	33:a41981e18a7d	1542	wait_ms(100);
Fairy_Paolina	33:a41981e18a7d	1543	while(motors.GetMotor0Current()<5 \|\| motors.GetMotor1Current() <5 );
Fairy_Paolina	33:a41981e18a7d	1544	motors.stopBothMotors(127);
Fairy_Paolina	33:a41981e18a7d	1545	wait_ms(300);
Fairy_Paolina	33:a41981e18a7d	1546
tashworth	17:a5bb85ee205d	1547	// Slight forward for turn
tashworth	20:55dcff40c5d9	1548	slightMove(FORWARD,150);
tashworth	17:a5bb85ee205d	1549	wait_ms(100);
Fairy_Paolina	23:f8e806d1ffcc	1550	leftTurn();
tashworth	28:2bb6b0fe39d0	1551
tashworth	28:2bb6b0fe39d0	1552
tashworth	14:784acd735b8c	1553	if(current > loc) {
tashworth	20:55dcff40c5d9	1554	//pc.printf("RIG section %f\r\n",current);
Fairy_Paolina	23:f8e806d1ffcc	1555	wall_follow2(LEFT, FORWARD, RIGS, current, rig);
tashworth	14:784acd735b8c	1556	current-=((abs(leftEncoder.getPulses()11.12/PPR) + abs(rightEncoder.getPulses()11.12/PPR))/2);
tashworth	14:784acd735b8c	1557	} else {
tashworth	20:55dcff40c5d9	1558	//pc.printf("RIG section %f\r\n",current);
Fairy_Paolina	23:f8e806d1ffcc	1559	wall_follow2(LEFT, BACKWARD, RIGS, current, rig);
tashworth	14:784acd735b8c	1560	current+=((abs(leftEncoder.getPulses()11.12/PPR) + abs(rightEncoder.getPulses()11.12/PPR))/2);
tashworth	14:784acd735b8c	1561	}
Fairy_Paolina	23:f8e806d1ffcc	1562	wait(1);
Fairy_Paolina	23:f8e806d1ffcc	1563	wall_follow2(LEFT, FORWARD, RIGS, current, rig);
Fairy_Paolina	23:f8e806d1ffcc	1564	current-=((abs(leftEncoder.getPulses()11.12/PPR) + abs(rightEncoder.getPulses()11.12/PPR))/2);
Fairy_Paolina	33:a41981e18a7d	1565	wait(1);
Fairy_Paolina	33:a41981e18a7d	1566
Fairy_Paolina	33:a41981e18a7d	1567	alignWithRig();
tashworth	14:784acd735b8c	1568	}
tashworth	28:2bb6b0fe39d0	1569
tashworth	14:784acd735b8c	1570	void tools_section_return(float* location, float &current)
tashworth	14:784acd735b8c	1571	{
tashworth	14:784acd735b8c	1572	if(location[0] > 16) {
tashworth	14:784acd735b8c	1573	leftTurn();
Fairy_Paolina	23:f8e806d1ffcc	1574	wall_follow2(LEFT, BACKWARD, TOOLS, location[0], 0);
tashworth	14:784acd735b8c	1575	}
Fairy_Paolina	23:f8e806d1ffcc	1576	motors.stopBothMotors(127);
tashworth	28:2bb6b0fe39d0	1577
tashworth	14:784acd735b8c	1578	}
tashworth	28:2bb6b0fe39d0	1579
tashworth	14:784acd735b8c	1580	void mid_section_return(float* location, float &current, int* direction)
tashworth	14:784acd735b8c	1581	{
tashworth	14:784acd735b8c	1582	if(direction[0] == RIGHT) {
tashworth	14:784acd735b8c	1583	rightTurn();
Fairy_Paolina	33:a41981e18a7d	1584	//alignWithWall(MID);
Fairy_Paolina	23:f8e806d1ffcc	1585	wall_follow2(LEFT, FORWARD, MID, current,0);
Fairy_Paolina	23:f8e806d1ffcc	1586	current-=((abs(leftEncoder.getPulses()11.12/PPR) + abs(rightEncoder.getPulses()11.12/PPR))/2);
Fairy_Paolina	23:f8e806d1ffcc	1587	slightMove(FORWARD, 50);
tashworth	14:784acd735b8c	1588	leftTurn();
Fairy_Paolina	23:f8e806d1ffcc	1589	} else if(direction[0] == LEFT) {
Fairy_Paolina	23:f8e806d1ffcc	1590	rightTurn();
Fairy_Paolina	23:f8e806d1ffcc	1591	wall_follow2(RIGHT, BACKWARD, MID, current,0);
tashworth	14:784acd735b8c	1592	current+=((abs(leftEncoder.getPulses()11.12/PPR) + abs(rightEncoder.getPulses()11.12/PPR))/2);
Fairy_Paolina	23:f8e806d1ffcc	1593	wait_ms(200);
Fairy_Paolina	23:f8e806d1ffcc	1594	slightMove(BACKWARD, 100);
Fairy_Paolina	23:f8e806d1ffcc	1595	leftTurn();
tashworth	14:784acd735b8c	1596	}
tashworth	14:784acd735b8c	1597	//ELSE and GO FORWARD
tashworth	14:784acd735b8c	1598	overBump(RIGS);
tashworth	14:784acd735b8c	1599	}
tashworth	28:2bb6b0fe39d0	1600
tashworth	14:784acd735b8c	1601	void mid_section2_return(float* location, float &current, int* direction)
tashworth	14:784acd735b8c	1602	{
tashworth	14:784acd735b8c	1603	if(direction[1] == RIGHT) {
Fairy_Paolina	23:f8e806d1ffcc	1604	rightTurn();
Fairy_Paolina	23:f8e806d1ffcc	1605	wall_follow2(LEFT, FORWARD, RETURN, current,0);
Fairy_Paolina	23:f8e806d1ffcc	1606	current-=((abs(leftEncoder.getPulses()11.12/PPR) + abs(rightEncoder.getPulses()11.12/PPR))/2);
tashworth	14:784acd735b8c	1607	leftTurn();
tashworth	14:784acd735b8c	1608	} else if(direction[1] == LEFT) {
Fairy_Paolina	23:f8e806d1ffcc	1609	rightTurn();
Fairy_Paolina	23:f8e806d1ffcc	1610	wall_follow2(LEFT, BACKWARD, RETURN, current,0);
Fairy_Paolina	23:f8e806d1ffcc	1611	current+=((abs(leftEncoder.getPulses()11.12/PPR) + abs(rightEncoder.getPulses()11.12/PPR))/2);
tashworth	14:784acd735b8c	1612	leftTurn();
tashworth	14:784acd735b8c	1613	}
tashworth	14:784acd735b8c	1614	//ELSE and GO FORWARD
tashworth	14:784acd735b8c	1615	overBump(MID);
tashworth	14:784acd735b8c	1616	}
tashworth	28:2bb6b0fe39d0	1617
tashworth	14:784acd735b8c	1618	void rig_section_return(float* location, float &current, int* direction)
tashworth	14:784acd735b8c	1619	{
Fairy_Paolina	33:a41981e18a7d	1620	//alignWithWall(RIGS);
tashworth	28:2bb6b0fe39d0	1621
tashworth	14:784acd735b8c	1622	if(location[2] > current) {
Fairy_Paolina	23:f8e806d1ffcc	1623	wall_follow2(LEFT, BACKWARD, RETURN, current,0);
tashworth	14:784acd735b8c	1624	current+=((abs(leftEncoder.getPulses()11.12/PPR) + abs(rightEncoder.getPulses()11.12/PPR))/2);
Fairy_Paolina	23:f8e806d1ffcc	1625	wait_ms(500);
Fairy_Paolina	23:f8e806d1ffcc	1626	//slightMove(FORWARD,50);
tashworth	14:784acd735b8c	1627	} else {
Fairy_Paolina	23:f8e806d1ffcc	1628	wall_follow2(LEFT, FORWARD, RETURN, current,0);
tashworth	14:784acd735b8c	1629	current-=((abs(leftEncoder.getPulses()11.12/PPR) + abs(rightEncoder.getPulses()11.12/PPR))/2);
tashworth	14:784acd735b8c	1630	}
tashworth	28:2bb6b0fe39d0	1631
Fairy_Paolina	23:f8e806d1ffcc	1632	// LEFT TURN
Fairy_Paolina	23:f8e806d1ffcc	1633	motors.begin();
Fairy_Paolina	23:f8e806d1ffcc	1634	leftEncoder.reset();
Fairy_Paolina	23:f8e806d1ffcc	1635	rightEncoder.reset();
Fairy_Paolina	23:f8e806d1ffcc	1636	motors.setMotor0Speed(0.5*127);// right
Fairy_Paolina	23:f8e806d1ffcc	1637	motors.setMotor1Speed(-0.5*127);// left
Fairy_Paolina	23:f8e806d1ffcc	1638	while(abs(leftEncoder.getPulses())<1100 \|\| rightEncoder.getPulses()<110);
tashworth	28:2bb6b0fe39d0	1639
Fairy_Paolina	23:f8e806d1ffcc	1640	motors.stopBothMotors(127);
tashworth	14:784acd735b8c	1641	overBump(MID2);
tashworth	20:55dcff40c5d9	1642	}
tashworth	28:2bb6b0fe39d0	1643
tashworth	28:2bb6b0fe39d0	1644
tashworth	28:2bb6b0fe39d0	1645
tashworth	28:2bb6b0fe39d0	1646
tashworth	20:55dcff40c5d9	1647	int Xadjust(int tool)
tashworth	20:55dcff40c5d9	1648	{
tashworth	20:55dcff40c5d9	1649	int areaArray[10];
tashworth	20:55dcff40c5d9	1650	float C, T, S;
tashworth	22:79c5871543b5	1651	for(int i = 0; i < 5; i++) {
tashworth	20:55dcff40c5d9	1652	areaArray[i] = shapeDetection();
tashworth	20:55dcff40c5d9	1653	wait(2);
tashworth	20:55dcff40c5d9	1654	if(get_com_x() > X_CENTER ) {
tashworth	20:55dcff40c5d9	1655	deltaX = get_com_x()-X_CENTER;
tashworth	20:55dcff40c5d9	1656	setServoPulse2(0,Arm_Table[tool].base_rotate += (deltaX/60.0) );
tashworth	20:55dcff40c5d9	1657	Arm_Table[PU_TOOL_2].base_rotate = Arm_Table[TOOL_2].base_rotate;
tashworth	20:55dcff40c5d9	1658	}
tashworth	20:55dcff40c5d9	1659	if(get_com_x() < X_CENTER) {
tashworth	20:55dcff40c5d9	1660	deltaX = get_com_x()-X_CENTER;
tashworth	20:55dcff40c5d9	1661	setServoPulse2(0,Arm_Table[tool].base_rotate += (deltaX/60.0) );
tashworth	20:55dcff40c5d9	1662	Arm_Table[PU_TOOL_2].base_rotate = Arm_Table[TOOL_2].base_rotate;
tashworth	20:55dcff40c5d9	1663	}
tashworth	28:2bb6b0fe39d0	1664
tashworth	28:2bb6b0fe39d0	1665
tashworth	20:55dcff40c5d9	1666	}
tashworth	28:2bb6b0fe39d0	1667
tashworth	20:55dcff40c5d9	1668	C = normd(areaArray, 10, CIRCLE_AREA);
tashworth	21:0907e1f5e16c	1669	// S = normd(areaArray, 10, SQUARE_AREA);
tashworth	21:0907e1f5e16c	1670	// T = normd(areaArray, 10, TRIANGLE_AREA);
tashworth	28:2bb6b0fe39d0	1671
tashworth	20:55dcff40c5d9	1672	if((C < SQUARE_AREA) && (C > CIRCLE_AREA)) {
tashworth	22:79c5871543b5	1673	pc.printf("\n\nCIRCLE DETECTED\n\r");
tashworth	22:79c5871543b5	1674	return CIRCLE;
tashworth	22:79c5871543b5	1675	} else if( ( C > SQUARE_AREA) ) {
tashworth	20:55dcff40c5d9	1676	pc.printf("\n\nSQUARE DETECTED\n\r");
tashworth	20:55dcff40c5d9	1677	return SQUARE;
tashworth	20:55dcff40c5d9	1678	} else {
tashworth	20:55dcff40c5d9	1679	pc.printf("\n\nTRIANGLE DETECTED\n\r");
tashworth	20:55dcff40c5d9	1680	return TRIANGLE;
tashworth	21:0907e1f5e16c	1681	}
tashworth	28:2bb6b0fe39d0	1682
tashworth	28:2bb6b0fe39d0	1683
tashworth	21:0907e1f5e16c	1684	/*
tashworth	21:0907e1f5e16c	1685	if((C < S) && (C < T)) {
tashworth	21:0907e1f5e16c	1686	pc.printf("\n\nCIRCLE DETECTED\n\r");
tashworth	21:0907e1f5e16c	1687	return CIRCLE;
tashworth	21:0907e1f5e16c	1688	} else if( ( S<C ) && ( S<T ) ) {
tashworth	21:0907e1f5e16c	1689	pc.printf("\n\nSQUARE DETECTED\n\r");
tashworth	21:0907e1f5e16c	1690	return SQUARE;
tashworth	21:0907e1f5e16c	1691	} else {
tashworth	21:0907e1f5e16c	1692	pc.printf("\n\nTRIANGLE DETECTED\n\r");
tashworth	21:0907e1f5e16c	1693	return TRIANGLE;
tashworth	21:0907e1f5e16c	1694	}*/
tashworth	20:55dcff40c5d9	1695	}
tashworth	28:2bb6b0fe39d0	1696
tashworth	20:55dcff40c5d9	1697	float normd(int* pop, int count, int threshold)
tashworth	20:55dcff40c5d9	1698	{
tashworth	20:55dcff40c5d9	1699	int i = 0;
tashworth	20:55dcff40c5d9	1700	float mean=0, std=0;
tashworth	20:55dcff40c5d9	1701	for(i=0; i<count; i++) {
tashworth	20:55dcff40c5d9	1702	mean += pop[i];
tashworth	20:55dcff40c5d9	1703	}
tashworth	20:55dcff40c5d9	1704	mean /= (float)count;
tashworth	20:55dcff40c5d9	1705	pc.printf("\n\nMean: %f\n\r", mean);
tashworth	28:2bb6b0fe39d0	1706
tashworth	20:55dcff40c5d9	1707	for(i=0; i<count; i++) {
tashworth	20:55dcff40c5d9	1708	std += pow(((float)pop[i]-mean),2);
tashworth	20:55dcff40c5d9	1709	}
tashworth	20:55dcff40c5d9	1710	std /= (float)count;
tashworth	20:55dcff40c5d9	1711	std = sqrt(std);
tashworth	20:55dcff40c5d9	1712	//pc.printf("\n\nStd: %f\n\r", std);
tashworth	28:2bb6b0fe39d0	1713
tashworth	20:55dcff40c5d9	1714	//pc.printf("\n\nNorm: %f\n\r", (1/(stdsqrt(2PI)))exp(-pow(((float)threshold-mean),2)/(2pow(std,2))));
tashworth	28:2bb6b0fe39d0	1715
tashworth	28:2bb6b0fe39d0	1716
tashworth	20:55dcff40c5d9	1717	//return abs(mean - threshold);
tashworth	20:55dcff40c5d9	1718	return mean;
tashworth	20:55dcff40c5d9	1719	//return (1/(stdsqrt(2PI)))exp(-pow(((float)threshold-mean),2)/(2pow(std,2)));
tashworth	28:2bb6b0fe39d0	1720
Fairy_Paolina	27:5540aa3c828c	1721	}
Fairy_Paolina	27:5540aa3c828c	1722
tashworth	28:2bb6b0fe39d0	1723	void testSensors(void)
tashworth	28:2bb6b0fe39d0	1724	{
Fairy_Paolina	27:5540aa3c828c	1725	float range, range2;
srsmitherman	31:99cf9c25b0f2	1726
tashworth	28:2bb6b0fe39d0	1727
tashworth	28:2bb6b0fe39d0	1728	while(1) {
srsmitherman	31:99cf9c25b0f2	1729
Fairy_Paolina	30:db07aea6d119	1730	//pc.printf("us front %f \t us right %f \t IR front %f \t IR back %f\r\n", range, range2, IRLeftFront.getIRDistance(), IRLeftBack.getIRDistance());
srsmitherman	31:99cf9c25b0f2	1731
Fairy_Paolina	30:db07aea6d119	1732	pc.printf("IR LEFT front %f \t",IRLeftFront.getIRDistance());
Fairy_Paolina	30:db07aea6d119	1733	pc.printf("IR left back %f \t",IRLeftBack.getIRDistance());
srsmitherman	31:99cf9c25b0f2	1734
Fairy_Paolina	30:db07aea6d119	1735	pc.printf("IR right front %f \t",IRRightFront.getIRDistance());
Fairy_Paolina	30:db07aea6d119	1736	pc.printf("IR right back %f \t",IRRightBack.getIRDistance());
Fairy_Paolina	30:db07aea6d119	1737
Fairy_Paolina	30:db07aea6d119	1738	pc.printf("IR front left %f \t",IRFrontL.getIRDistance());
Fairy_Paolina	30:db07aea6d119	1739	pc.printf("IR front right %f \r\n",IRFrontR.getIRDistance());
Fairy_Paolina	27:5540aa3c828c	1740	wait_ms(20);
Fairy_Paolina	27:5540aa3c828c	1741	}
srsmitherman	31:99cf9c25b0f2	1742	}
srsmitherman	31:99cf9c25b0f2	1743
srsmitherman	31:99cf9c25b0f2	1744
srsmitherman	31:99cf9c25b0f2	1745	void alignWithGap()
srsmitherman	31:99cf9c25b0f2	1746	{
srsmitherman	31:99cf9c25b0f2	1747	pc.printf("\r\n Align with gap.");
Fairy_Paolina	33:a41981e18a7d	1748
srsmitherman	31:99cf9c25b0f2	1749	wait(1);
Fairy_Paolina	33:a41981e18a7d	1750
Fairy_Paolina	33:a41981e18a7d	1751	if(IRLeftBack.getIRDistance() > 30) {
srsmitherman	31:99cf9c25b0f2	1752	motors.setMotor0Speed(-0.1*127);
srsmitherman	31:99cf9c25b0f2	1753	motors.setMotor1Speed(-0.1*127);
Fairy_Paolina	33:a41981e18a7d	1754	while(IRLeftBack.getIRDistance() > 30);
Fairy_Paolina	33:a41981e18a7d	1755	pc.printf("IR left back %f \t",IRLeftBack.getIRDistance());
srsmitherman	31:99cf9c25b0f2	1756
Fairy_Paolina	33:a41981e18a7d	1757	motors.stopBothMotors(127);
Fairy_Paolina	33:a41981e18a7d	1758	wait_ms(300);
Fairy_Paolina	33:a41981e18a7d	1759	}
srsmitherman	31:99cf9c25b0f2	1760
Fairy_Paolina	33:a41981e18a7d	1761
Fairy_Paolina	33:a41981e18a7d	1762	if(IRLeftBack.getIRDistance() < 30 && IRLeftFront.getIRDistance() > 30) {
srsmitherman	31:99cf9c25b0f2	1763	motors.setMotor0Speed(0.1*127);
srsmitherman	31:99cf9c25b0f2	1764	motors.setMotor1Speed(0.1*127);
Fairy_Paolina	33:a41981e18a7d	1765	while(IRLeftBack.getIRDistance() < 30);
srsmitherman	31:99cf9c25b0f2	1766
Fairy_Paolina	33:a41981e18a7d	1767	pc.printf("IR left back %f \t\r\n",IRLeftBack.getIRDistance());
Fairy_Paolina	33:a41981e18a7d	1768	motors.stopBothMotors(127);
Fairy_Paolina	33:a41981e18a7d	1769	wait_ms(300);
Fairy_Paolina	33:a41981e18a7d	1770	}
srsmitherman	31:99cf9c25b0f2	1771
srsmitherman	31:99cf9c25b0f2	1772	leftEncoder.reset();
srsmitherman	31:99cf9c25b0f2	1773	rightEncoder.reset();
srsmitherman	31:99cf9c25b0f2	1774
srsmitherman	31:99cf9c25b0f2	1775	motors.setMotor0Speed(0.1*127);
srsmitherman	31:99cf9c25b0f2	1776	motors.setMotor1Speed(0.1*127);
srsmitherman	31:99cf9c25b0f2	1777
Fairy_Paolina	33:a41981e18a7d	1778	while(abs(leftEncoder.getPulses())<575 \|\| rightEncoder.getPulses()<575);
srsmitherman	31:99cf9c25b0f2	1779
srsmitherman	31:99cf9c25b0f2	1780	motors.stopBothMotors(127);
Fairy_Paolina	33:a41981e18a7d	1781	wait_ms(300);
Fairy_Paolina	33:a41981e18a7d	1782	}
Fairy_Paolina	33:a41981e18a7d	1783
Fairy_Paolina	33:a41981e18a7d	1784	void alignWithRig(){
Fairy_Paolina	33:a41981e18a7d	1785
Fairy_Paolina	33:a41981e18a7d	1786	if(IRLeftFront.getIRDistance() < 30) {
Fairy_Paolina	33:a41981e18a7d	1787	motors.setMotor0Speed(0.1*127);
Fairy_Paolina	33:a41981e18a7d	1788	motors.setMotor1Speed(0.1*127);
Fairy_Paolina	33:a41981e18a7d	1789	while(IRLeftFront.getIRDistance() < 30);
Fairy_Paolina	33:a41981e18a7d	1790	pc.printf("IR left back %f \t",IRLeftFront.getIRDistance());
srsmitherman	31:99cf9c25b0f2	1791
Fairy_Paolina	33:a41981e18a7d	1792	motors.stopBothMotors(127);
Fairy_Paolina	33:a41981e18a7d	1793	wait_ms(300);
Fairy_Paolina	33:a41981e18a7d	1794	}
Fairy_Paolina	33:a41981e18a7d	1795
Fairy_Paolina	33:a41981e18a7d	1796	motors.setMotor0Speed(0.1*127);
Fairy_Paolina	33:a41981e18a7d	1797	motors.setMotor1Speed(0.1*127);
Fairy_Paolina	33:a41981e18a7d	1798
Fairy_Paolina	33:a41981e18a7d	1799	while(abs(leftEncoder.getPulses())<100 \|\| rightEncoder.getPulses()<100);
Fairy_Paolina	33:a41981e18a7d	1800
Fairy_Paolina	33:a41981e18a7d	1801	motors.stopBothMotors(127);
Fairy_Paolina	33:a41981e18a7d	1802	wait_ms(300);
Fairy_Paolina	33:a41981e18a7d	1803
Fairy_Paolina	33:a41981e18a7d	1804
Fairy_Paolina	33:a41981e18a7d	1805
tashworth	22:79c5871543b5	1806	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Created:	12 Apr 2014
Imports:	11
Forks:	0
Commits:	35
Dependents:	0
Dependencies:	7
Followers:	6

main.cpp@33:a41981e18a7d, 2014-04-22 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning