theRobot3 - uyvug

UH Robotics » Code » theRobot3

uyvug

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

main.cpp@26:7257bd16bc67, 2014-04-14 (annotated)

Committer:: Fairy_Paolina
Date:: Mon Apr 14 12:17:10 2014 +0000
Revision:: 26:7257bd16bc67
Parent:: 24:3369d51f6cbd
Child:: 27:5540aa3c828c

comment out, skipping the first section;

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

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@26:7257bd16bc67, 2014-04-14 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning