theRobotNEW - ksdflsjdfljas

Users » Fairy_Paolina » Code » theRobotNEW

ksdflsjdfljas

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

main.cpp@16:8bb212df81b7, 2014-04-02 (annotated)

Committer:: tashworth
Date:: Wed Apr 02 23:54:05 2014 +0000
Revision:: 16:8bb212df81b7
Parent:: 14:784acd735b8c
Child:: 17:a5bb85ee205d

4-2-14

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

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Created:	03 Apr 2014
Imports:	1
Forks:	0
Commits:	22
Dependents:	0
Dependencies:	7
Followers:	1

main.cpp@16:8bb212df81b7, 2014-04-02 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning