theRobotNEW - ksdflsjdfljas

Users » Fairy_Paolina » Code » theRobotNEW

ksdflsjdfljas

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

main.cpp@12:284be46593ae, 2014-04-02 (annotated)

Committer:: tashworth
Date:: Wed Apr 02 00:30:30 2014 +0000
Revision:: 12:284be46593ae
Parent:: 11:8d2455e383ce
Child:: 13:529323807361

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

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@12:284be46593ae, 2014-04-02 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning