theRobot - nav fixed

Users » jjcarr2 » Code » theRobot

nav fixed

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

main.cpp@22:072dabdf905a, 2014-04-11 (annotated)

Committer:: jjcarr2
Date:: Fri Apr 11 20:56:52 2014 +0000
Revision:: 22:072dabdf905a
Parent:: 21:0907e1f5e16c

cleaned up navigation;

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

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Created:	11 Apr 2014
Imports:	1
Forks:	0
Commits:	23
Dependents:	0
Dependencies:	7
Followers:	1

main.cpp@22:072dabdf905a, 2014-04-11 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning