theRobot3 - uyvug

UH Robotics » Code » theRobot3

uyvug

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

main.cpp@23:f8e806d1ffcc, 2014-04-12 (annotated)

Committer:: Fairy_Paolina
Date:: Sat Apr 12 20:27:00 2014 +0000
Revision:: 23:f8e806d1ffcc
Parent:: 22:79c5871543b5
Child:: 24:3369d51f6cbd

added current stuff;

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

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@23:f8e806d1ffcc, 2014-04-12 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning