Thomas Ashworth
For IEEE Robotics
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
main.cpp
- Committer:
- tashworth
- Date:
- 2014-04-03
- Revision:
- 17:a5bb85ee205d
- Parent:
- 16:8bb212df81b7
- Child:
- 18:a0ea7ecaf4fe
File content as of revision 17:a5bb85ee205d:
#include "mbed.h"
#include "Adafruit_PWMServoDriver.h"
#include "ShapeDetect.h"
#include "rtos.h"
#include "PID.h"
#include "PololuQik2.h"
#include "QEI.h"
#include "HCSR04.h"
#include "stdio.h"
#include "LPC17xx.h"
#include "Sharp.h"
/* Navigation Definitions */
#define PIN_TRIGGERL (p12)
#define PIN_ECHOL (p11)
#define PIN_TRIGGERR (p29)
#define PIN_ECHOR (p30)
#define PULSE_PER_REV (1192)
#define WHEEL_CIRCUM (12.56637)
#define DIST_PER_PULSE (0.01054225722682)
#define MTRS_TO_INCH (39.3701)
#define MAX_SPEED (0.3*127)
#define PPR (4331/4)
#define LEFT (1)
#define RIGHT (0)
#define STRAIGHT (2)
#define FORWARD (1)
#define BACKWARD (0)
#define TOOLS (0)
#define MID (1)
#define RIGS (2)
#define MID2 (3)
#define RETURN (4)
#define FAR (1)
//States
#define START 0
#define OILRIG1_POS 1
#define OILRIG2_POS 2
#define GOTO_TOOLS1 3
#define IDENTIFY_TOOLS 4
#define AQUIRE_TOOL1 5
#define AQUIRE_TOOL2 6
#define AQUIRE_TOOL3 7
#define NAVIGATE_WAVES_ROW1 8
#define NAVIGATE_WAVES_ROW2 9
#define NAVIGATE_WAVES_ROW3 10
#define NAVIGATE_TO_SQUARE_RIG 11
#define NAVIGATE_TO_TRIANGLE_RIG 12
#define NAVIGATE_TO_CIRCLE_RIG 13
#define RIG_ALIGN 14
#define INSERT_TOOL 15
#define END 16
#define GOTO_TOOLS2 17
//Servo Static Positions
#define STORE_POSITION 0
#define OIL_RIG1 1
#define OIL_RIG2 2
#define OIL_RIG3 3
#define DRIVE_POSITION_NOTOOL 4
#define TOOL_1 5
#define TOOL_2 6
#define TOOL_3 7
#define DRIVE_POSITION_TOOL 8
#define ORIENT_TOOL 9
#define PU_TOOL_1 10
#define PU_TOOL_2 11
#define PU_TOOL_3 12
#define PU_TOOL_1_STAB 13
#define PU_TOOL_2_STAB 14
#define PU_TOOL_3_STAB 15
//Rig definitions
#define SQUARE 1
#define TRIANGLE 2
#define CIRCLE 3
//Oil Rig distance thresholds
#define OILRIG1_MAX 2200
#define OILRIG1_MIN 1000
#define OILRIG2_MAX 2200
#define OILRIG2_MIN 1000
#define OILRIG3_MAX 2200
#define OILRIG3_MIN 1000
//for servo normalization
#define MIN_SERVO_PULSE 900
#define MAX_SERVO_PULSE 2100
#define SERVO_MAX_ANGLE 180
#define X_CENTER 80
#define Y_CENTER 60
DigitalOut myled1(LED1);
DigitalOut myled2(LED2);
DigitalOut myled3(LED3);
DigitalOut myled4(LED4);
InterruptIn startBtn(p7);
void errFunction(void);
bool cRc;
Serial pc(USBTX,USBRX); //USB Comm
Adafruit_PWMServoDriver pwm(p28,p27); //pwm(SDA,SCL) - Servo Control PWM
DigitalOut ServoOutputDisable(p8); //Servo Control Output Enable/Disable
extern Serial lrf; //Laser Range Finder lrf(p13,p14)
//Hardware Initialization
//Serial bt(p13,p14);
HCSR04 rangeFinderLeft( PIN_TRIGGERL, PIN_ECHOL );
HCSR04 rangeFinderRight( PIN_TRIGGERR, PIN_ECHOR );
PID pid1(15.0,0.0,4.0,0.02);
PololuQik2 motors(p9, p10, p8, p15, errFunction, cRc);
QEI rightEncoder(p17,p18,NC,PPR,QEI::X4_ENCODING);
QEI leftEncoder(p16,p15,NC,PPR,QEI::X4_ENCODING);
Sharp IR(p20);
//InterruptIn encoder(p29);
/***************
local servo functions
****************/
void servoBegin(void);
void initServoDriver(void);
void setServoPulse(uint8_t n, int angle);
void setServoPulse2(uint8_t n, float angle); //float precision
void setServoPulseNo_delay(uint8_t n, int angle);
void servoPosition(int set);
int fire_checker(int rig);
int button_start = 0;
//Navigation Functions
void wall_follow(int side, int direction, int section);
void wall_follow2(int side, int direction, int section, float location, int rig);
void leftTurn(void);
void slightleft(void);
void slightright(void);
void rightTurn(void);
void slightMove(int direction, float pulses);
void to_tools_section1(float* location, float ¤t);
void to_tools_section2(float* location, float ¤t);
void from_tools_section(float* location, float ¤t);
void mid_section(float* location, float ¤t, int* direction);
void mid_section2(float* location, float ¤t, int* direction);
void rig_section(float* location, float ¤t, int* direction, int rig);
void tools_section_return(float* location, float ¤t);
void mid_section_return(float* location, float ¤t, int* direction);
void mid_section2_return(float* location, float ¤t, int* direction);
void rig_section_return(float* location, float ¤t, int* direction);
void overBump(int section);
void alignWithWall(int section);
void UntilWall(int dir);
extern "C" void mbed_reset();
/************
Main Variables
*************/
int state = START;
int fire = 0;
int tool_needed = 0;
int shape_detected = 0;
float range, range2, pid_return;
/************
Variables for Servos
*************/
int servoNum, servoAngle, outputDisabled, posNum, testVal;
int currentPosition[7];
typedef struct {
int arm_position_name; //for organization only (STORE, OILRIG1, OILRIG2...)
int base_rotate;
int base_arm;
int big_arm;
int claw_arm;
int claw_rotate;
int claw_open;
} Coord;
/********************
Static Arm Positions
*********************/
Coord Arm_Table[] = {
// POSITION ODER:
// base_rotate, base_arm, int big_arm, int claw_arm, int claw_rotate, int claw_open
//increase in number 5 rotates gripper
{STORE_POSITION, 85, 10, 0, 165, 175, 0}, // storing position
{OIL_RIG1, 160, 20, 60, 45, 175, 0}, // point laser at oilrig1
{OIL_RIG2, 163, 20, 60, 45, 175, 0}, // point laser at oilrig2
{OIL_RIG3, 130, 90, 90, 52, 175, 0}, // point laser at oilrig2
{DRIVE_POSITION_NOTOOL, 85, 10, 0, 165, 175, 0}, // Drive through course
{TOOL_1, 96, 50, 80, 109, 90, 0}, // Look over first tool
{TOOL_2, 79, 50, 80, 110, 90, 0}, // Look over second tool
{TOOL_3, 59, 50, 80, 109, 90, 0}, // Look over third tool
{DRIVE_POSITION_TOOL, 85, 10, 0, 165, 90, 105}, // Drive with tool loaded
{ORIENT_TOOL, 135, 60, 75, 60, 90, 90}, // position tool to be inserted
{PU_TOOL_1, 96, 46, 78, 102, 170, 0}, // STATIC Pickup tool POSITION
{PU_TOOL_2, 74, 47, 80, 104, 170, 0}, // STATIC Pickup tool POSITION
{PU_TOOL_3, 57, 44, 76, 102, 170, 0}, // STATIC Pickup tool POSITION
{PU_TOOL_1_STAB, 98, 50, 90, 118, 90, 0}, // STAB TOOL 1
{PU_TOOL_2_STAB, 78, 50, 90, 108, 90, 0}, // STAB TOOL 2
{PU_TOOL_3_STAB, 53, 50, 90, 118, 90, 0}, // STAB TOOL 3
};
/* Variables for imageprocessing and distance */
int x_coord;
int y_coord;
int area;
int shape;
int shape_alignX_done = 0;
int shape_alignY_done = 0;
float deltaX = 0;
/* Variables for distance sensor*/
int distLaser;
int fire_detected = 0;
int fire_not_detected = 0;
void button_int(void)
{
if(!button_start) {
button_start = 1;
wait(1.0);
} else {
button_start = 0;
mbed_reset();
}
return;
}
int main()
{
/*****************
INITIALIZATIONS
*******************/
float location[3], current=4;
int direction[3];
double distance;
int pu, num, input;
pc.baud(115200);
//Laser Range Finder Initialization
lrf_baudCalibration();
motors.begin();
startBtn.rise(&button_int);
//Servo initialization
initServoDriver();
servoBegin(); //initiates servos to start position
//ServoOutputDisable = 0;
/********************************
MAIN WHILE LOOP FOR COMPETITION
*********************************/
while(1) {
if(button_start == 1) {
switch (state) {
/**************************************************
* STAGE 0
*
* - START OF THE COMETITION
*
**************************************************/
case START :
myled1 = 1;
state = OILRIG1_POS;
break;
/**************************************************
* STAGE 1
*
* - DETERMINE OIL RIG ON FIRE
*
**************************************************/
case OILRIG1_POS: //aims arm at square oil rig
servoPosition(OIL_RIG1);
wait(3); //wait for servo to settle before laser distance
fire = fire_checker(OIL_RIG1); //determines if oil rig is on fire
//determines what tool is needed
if (fire == 1) {
pc.printf("FIRE FOUND!!!!!!!!\n\r");
tool_needed = SQUARE;
state = GOTO_TOOLS1;
} else {
pc.printf("XXXXXX FIRE NOT FOUND XXXXXX");
state = OILRIG2_POS;
}
break;
case OILRIG2_POS:
setServoPulse(2, Arm_Table[DRIVE_POSITION_NOTOOL].big_arm);
setServoPulse(3, Arm_Table[DRIVE_POSITION_NOTOOL].claw_arm);
setServoPulse(1, Arm_Table[DRIVE_POSITION_NOTOOL].base_arm);
setServoPulse(0, Arm_Table[DRIVE_POSITION_NOTOOL].base_rotate);
setServoPulse(4, Arm_Table[DRIVE_POSITION_NOTOOL].claw_rotate);
setServoPulse(5, Arm_Table[DRIVE_POSITION_NOTOOL].claw_open);
wait(3); //wait for servos to settle
to_tools_section2(location, current); // moves to second rig
servoPosition(OIL_RIG2); //position arm to point at first oilrig
wait(3);
fire = fire_checker(OIL_RIG2);
if (fire == 1) {
pc.printf("FIRE FOUND!!!!!!!!");
tool_needed = TRIANGLE;
state = GOTO_TOOLS2;
} else {
pc.printf("XXXXXX FIRE NOT FOUND XXXXXX");
tool_needed = CIRCLE;
state = GOTO_TOOLS2;
}
break;
/**************************************************
* STAGE 2
*
* - TRAVEL TO TOOLS
*
**************************************************/
case GOTO_TOOLS1:
setServoPulse(2, Arm_Table[DRIVE_POSITION_NOTOOL].big_arm);
setServoPulse(3, Arm_Table[DRIVE_POSITION_NOTOOL].claw_arm);
setServoPulse(1, Arm_Table[DRIVE_POSITION_NOTOOL].base_arm);
setServoPulse(0, Arm_Table[DRIVE_POSITION_NOTOOL].base_rotate);
setServoPulse(4, Arm_Table[DRIVE_POSITION_NOTOOL].claw_rotate);
setServoPulse(5, Arm_Table[DRIVE_POSITION_NOTOOL].claw_open);
wait(3); //wait for servos to settle
to_tools_section1(location, current);
state = IDENTIFY_TOOLS;
break;
case GOTO_TOOLS2:
setServoPulse(2, Arm_Table[DRIVE_POSITION_NOTOOL].big_arm);
setServoPulse(3, Arm_Table[DRIVE_POSITION_NOTOOL].claw_arm);
setServoPulse(1, Arm_Table[DRIVE_POSITION_NOTOOL].base_arm);
setServoPulse(0, Arm_Table[DRIVE_POSITION_NOTOOL].base_rotate);
setServoPulse(4, Arm_Table[DRIVE_POSITION_NOTOOL].claw_rotate);
setServoPulse(5, Arm_Table[DRIVE_POSITION_NOTOOL].claw_open);
wait(3); //wait for servos to settle
slightMove(FORWARD,3100);
current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
state = IDENTIFY_TOOLS;
break;
/**************************************************
* STAGE 3
*
* - Determine order of tools
* - Aquire appropriate tool
*
**************************************************/
case IDENTIFY_TOOLS:
wait(5);
servoPosition(TOOL_2); //arm/camera looks over tool
wait(5); //wait for servos to settle
//shape_detected = shapeDetection(); //determines the shape
//clearBounds();
//printImageToFile(BINARY);
shape_alignX_done = 0;
shape_alignY_done = 0;
/*
while( shape_alignY_done == 0) {
wait(1);
shape_detected = shapeDetection();
pc.printf("Y - Adjust to center tool\n\r");
if(get_com_y() < 50) {
wait(1);
slightMove(FORWARD,25);
current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
} else if(get_com_y() > 70) {
wait(1);
slightMove(BACKWARD,25);
current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
} else {
shape_alignY_done = 1;
}
}*/
for(int i = 0; i < 4; i++) {
shape_detected = shapeDetection();
wait(2);
if(get_com_x() > X_CENTER ) {
deltaX = get_com_x()-X_CENTER;
setServoPulse2(0,Arm_Table[TOOL_2].base_rotate += (deltaX/60.0) );
Arm_Table[PU_TOOL_2].base_rotate = Arm_Table[TOOL_2].base_rotate;
}
if(get_com_x() < X_CENTER) {
deltaX = get_com_x()-X_CENTER;
setServoPulse2(0,Arm_Table[TOOL_2].base_rotate += (deltaX/60.0) );
Arm_Table[PU_TOOL_2].base_rotate = Arm_Table[TOOL_2].base_rotate;
}
}
/*
while( shape_alignX_done == 0) {
wait(1);
shape_detected = shapeDetection();
pc.printf("X - Adjust to center tool\n\r");
if(get_com_x() > 95) {
setServoPulse(0,Arm_Table[TOOL_2].base_rotate+=1);
} else if(get_com_x() < 65) {
setServoPulse(0,Arm_Table[TOOL_2].base_rotate-=1);
} else {
shape_alignX_done = 1;
}
}*/
//printImageToFile(BINARY);
//either goes to aquire the tool or look at the next shape
if(shape_detected == tool_needed) {
state = AQUIRE_TOOL2;
break;
} else {
slightMove(FORWARD,40);
current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
servoPosition(TOOL_1);
wait(5); //wait for servos to settle
shape_alignX_done = 0;
shape_alignY_done = 0;
/*
while( shape_alignY_done == 0) {
wait(1);
shape_detected = shapeDetection();
pc.printf("Y - Adjust to center tool\n\r");
if(get_com_y() < 50) {
wait(1);
slightMove(FORWARD,25);
current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
} else if(get_com_y() > 70) {
wait(1);
slightMove(BACKWARD,25);
current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
} else {
shape_alignY_done = 1;
}
}*/
for(int i = 0; i < 4; i++) {
shape_detected = shapeDetection();
wait(2);
if (get_com_x() > X_CENTER) {
deltaX = get_com_x()-X_CENTER;
setServoPulse2(0,Arm_Table[TOOL_1].base_rotate += (deltaX/60.0) );
Arm_Table[PU_TOOL_1].base_rotate = Arm_Table[TOOL_1].base_rotate;
}
if (get_com_x() < X_CENTER) {
deltaX = get_com_x()-X_CENTER;
setServoPulse2(0,Arm_Table[TOOL_1].base_rotate += (deltaX/60.0) );
Arm_Table[PU_TOOL_1].base_rotate = Arm_Table[TOOL_1].base_rotate;
}
}
/*
while( shape_alignX_done == 0) {
wait(1);
shape_detected = shapeDetection();
pc.printf("X - Adjust to center tool\n\r");
if(get_com_x() > 100) {
setServoPulse(0,Arm_Table[TOOL_1].base_rotate+=1);
} else if(get_com_x() < 60) {
setServoPulse(0,Arm_Table[TOOL_1].base_rotate-=1);
} else {
shape_alignX_done = 1;
}
}*/
if (shape_detected == tool_needed) {
state = AQUIRE_TOOL1;
break;
} else {
servoPosition(TOOL_3);
wait(3); //wait for servos to settle
state = AQUIRE_TOOL3;
}
}
break;
case AQUIRE_TOOL1:
servoPosition(PU_TOOL_1);
setServoPulse(4, 130);
wait(5);
setServoPulse(0, Arm_Table[PU_TOOL_1].base_rotate );
wait(1);
setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm - 5);
wait(1);
setServoPulse(5, 105);
wait(.5);
setServoPulse(5, 00);
setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm - 6);
wait(.5);
setServoPulse(5, 105);
wait(.5);
setServoPulse(5, 00);
setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm - 10);
wait(1);
setServoPulse(5, 105);
wait(1);
setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm);
wait(2);
setServoPulse(2, Arm_Table[DRIVE_POSITION_TOOL].big_arm);
setServoPulse(3, Arm_Table[DRIVE_POSITION_TOOL].claw_arm);
setServoPulse(1, Arm_Table[DRIVE_POSITION_TOOL].base_arm);
setServoPulse(0, Arm_Table[DRIVE_POSITION_TOOL].base_rotate);
setServoPulse(4, Arm_Table[DRIVE_POSITION_TOOL].claw_rotate);
setServoPulse(5, Arm_Table[DRIVE_POSITION_TOOL].claw_open);
state = NAVIGATE_WAVES_ROW1;
break;
case AQUIRE_TOOL2:
servoPosition(PU_TOOL_2);
setServoPulse(4, 175);
wait(5);
setServoPulse(0, Arm_Table[PU_TOOL_2].base_rotate );
wait(1);
setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 2);
wait(1);
setServoPulse(3, Arm_Table[PU_TOOL_2].claw_arm - 2);
wait(1);
setServoPulse(5, 105);
wait(2);
setServoPulse(5, 00);
setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 6);
wait(2);
setServoPulse(5, 105);
wait(2);
setServoPulse(5, 00);
setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 10);
wait(2);
setServoPulse(5, 105);
wait(2);
setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm);
wait(2);
setServoPulse(2, Arm_Table[DRIVE_POSITION_TOOL].big_arm);
setServoPulse(3, Arm_Table[DRIVE_POSITION_TOOL].claw_arm);
setServoPulse(1, Arm_Table[DRIVE_POSITION_TOOL].base_arm);
setServoPulse(0, Arm_Table[DRIVE_POSITION_TOOL].base_rotate);
setServoPulse(4, Arm_Table[DRIVE_POSITION_TOOL].claw_rotate);
setServoPulse(5, Arm_Table[DRIVE_POSITION_TOOL].claw_open);
state = NAVIGATE_WAVES_ROW1;
break;
case AQUIRE_TOOL3:
/*
while( shape_alignY_done == 0) {
wait(1);
servoPosition(PU_TOOL_3);
shape_detected = shapeDetection();
wait(2);
pc.printf("Y - Adjust to center tool\n\r");
if(get_com_y() < 50) {
wait(1);
slightMove(FORWARD,25);
current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
} else if(get_com_y() > 70) {
wait(1);
slightMove(BACKWARD,25);
current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
} else {
shape_alignY_done = 1;
}
}
*/
for(int i = 0; i < 4; i++) {
shape_detected = shapeDetection();
wait(2);
if (get_com_x() > X_CENTER) {
deltaX = get_com_x()-X_CENTER;
setServoPulse2(0,Arm_Table[PU_TOOL_3].base_rotate += (deltaX/60.0) );
}
if (get_com_x() < X_CENTER) {
deltaX = get_com_x()-X_CENTER;
setServoPulse2(0,Arm_Table[PU_TOOL_3].base_rotate += (deltaX/60.0) );
}
}
setServoPulse(4, 130);
wait(5);
setServoPulse(0, Arm_Table[PU_TOOL_3].base_rotate );
wait(1);
setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 2);
wait(1);
setServoPulse(3, Arm_Table[PU_TOOL_2].claw_arm - 3);
wait(1);
setServoPulse(5, 105);
wait(.5);
setServoPulse(5, 00);
setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm - 6);
wait(.5);
setServoPulse(5, 105);
wait(.5);
setServoPulse(5, 00);
setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm - 10);
wait(1);
setServoPulse(5, 105);
wait(1);
setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm);
wait(2);
setServoPulse(2, Arm_Table[DRIVE_POSITION_TOOL].big_arm);
setServoPulse(3, Arm_Table[DRIVE_POSITION_TOOL].claw_arm);
setServoPulse(1, Arm_Table[DRIVE_POSITION_TOOL].base_arm);
setServoPulse(0, Arm_Table[DRIVE_POSITION_TOOL].base_rotate);
setServoPulse(4, Arm_Table[DRIVE_POSITION_TOOL].claw_rotate);
setServoPulse(5, Arm_Table[DRIVE_POSITION_TOOL].claw_open);
state = NAVIGATE_WAVES_ROW1;
break;
/**************************************************
* STAGE 4
*
* - Navigate through the ocean
*
**************************************************/
case NAVIGATE_WAVES_ROW1:
from_tools_section(location,current);
mid_section(location, current, direction);
state = NAVIGATE_WAVES_ROW2;
break;
case NAVIGATE_WAVES_ROW2:
mid_section2(location, current, direction);
state = NAVIGATE_WAVES_ROW3;
break;
case NAVIGATE_WAVES_ROW3:
if(shape_detected == 1) {
rig_section(location, current, direction, 1);
while(1);
state = NAVIGATE_TO_SQUARE_RIG;
} else if(shape_detected == 2) {
rig_section(location, current, direction, 2);
while(1);
state = NAVIGATE_TO_TRIANGLE_RIG;
} else {
rig_section(location, current, direction, 3);
while(1);
state = NAVIGATE_TO_CIRCLE_RIG;
}
break;
/**************************************************
* STAGE 5
*
* - Travel to appropriate rig
*
**************************************************/
case NAVIGATE_TO_SQUARE_RIG:
//NAVIGATION CODE HERE
state = RIG_ALIGN;
break;
case NAVIGATE_TO_TRIANGLE_RIG:
//NAVIGATION CODE HERE
state = RIG_ALIGN;
break;
case NAVIGATE_TO_CIRCLE_RIG:
//NAVIGATION CODE HERE
state = RIG_ALIGN;
break;
/**************************************************
* STAGE 6
*
* - Align with appropriate rig
*
**************************************************/
case RIG_ALIGN:
//*********************//
//******* TODO ********//
//*********************//
// CODE TO ALIGN ROBOT WITH RIG
servoPosition(ORIENT_TOOL);
wait(1); //wait for servos to settle
state = INSERT_TOOL;
break;
/**************************************************
* STAGE 7
*
* - Insert Tool
* - Extenguish fire
* - win contest
*
**************************************************/
case INSERT_TOOL:
//*********************//
//******* TODO ********//
//*********************//
// CODE TO INSERT TOOL
break;
/**************************************************
* STAGE 8
*
* - END COMPETITION
*
**************************************************/
case END:
servoPosition(STORE_POSITION);
myled1 = 1;
wait(.2);
myled2 = 1;
wait(.2);
myled3 = 1;
wait(.2);
myled4 = 1;
wait(.2);
break;
default:
break;
}
} // End while loop
} // End if for start button
} // main loop
/************
Servo Functions
**************/
void setServoPulse(uint8_t n, int angle)
{
int pulse;
pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE - MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE);
float pulselength = 20000; // 20,000 us per second
int i = currentPosition[n];
pc.printf("ServoNumber: %d Begining Pulse: %d\n\r",n,currentPosition[n]);
int pulse2;
if(currentPosition[n] < pulse) {
for(i; i < pulse; i++) {
pulse2 = 4094 * i / pulselength;
pwm.setPWM(n, 0, pulse2);
wait_ms(3);
}
} else if (currentPosition[n] > pulse) {
for(i; i > pulse; i--) {
pulse2 = 4094 * i / pulselength;
pwm.setPWM(n, 0, pulse2);
wait_ms(3);
}
}
currentPosition[n] = i;
pc.printf("END: pulse: %d, angle: %d\n\r", i, angle);
}
void initServoDriver(void)
{
pwm.begin();
//pwm.setPWMFreq(100); //This dosen't work well because of uncertain clock speed. Use setPrescale().
pwm.setPrescale(140); //This value is decided for 20ms interval.
pwm.setI2Cfreq(400000); //400kHz
}
void servoBegin(void)
{
pc.printf("Setting Initial Servo Position\n\r");
setServoPulseNo_delay(3, Arm_Table[STORE_POSITION].claw_arm);
setServoPulseNo_delay(2, Arm_Table[STORE_POSITION].big_arm);
wait(2);
setServoPulseNo_delay(1, Arm_Table[STORE_POSITION].base_arm);
setServoPulseNo_delay(0, Arm_Table[STORE_POSITION].base_rotate);
setServoPulseNo_delay(4, Arm_Table[STORE_POSITION].claw_rotate);
setServoPulseNo_delay(5, Arm_Table[STORE_POSITION].claw_open);
}
void setServoPulseNo_delay(uint8_t n, int angle)
{
int pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE - MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE);
float pulselength = 20000; // 20,000 us per second
currentPosition[n] = pulse;
//pc.printf("ServoNumber: %d Pulsewidth: %d Angle: %d \n\r",n,pulse, angle);
pulse = 4094 * pulse / pulselength;
pwm.setPWM(n, 0, pulse);
}
void setServoPulse2(uint8_t n, float angle)
{
float pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE - MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE);
float pulselength = 20000; // 10,000 us per second
pulse = 4094 * pulse / pulselength;
pwm.setPWM(n, 0, pulse);
}
void servoPosition(int set)
{
//moves to current position
setServoPulse(3, Arm_Table[set].claw_arm);
setServoPulse(2, Arm_Table[set].big_arm);
setServoPulse(1, Arm_Table[set].base_arm);
setServoPulse(0, Arm_Table[set].base_rotate);
setServoPulse(4, Arm_Table[set].claw_rotate);
setServoPulse(5, Arm_Table[set].claw_open);
}
int fire_checker(int rig)
{
switch (rig) {
case 1:
for (int i = 0; i<10; i++) {
distLaser = getDistance();
pc.printf("L DISTANCE: %d \n\r", distLaser);
if ((distLaser < OILRIG1_MAX)
&& (distLaser > OILRIG1_MIN)) {
fire_detected++;
} else {
fire_not_detected++;
}
}
break;
case 2:
for (int i = 0; i<10; i++) {
distLaser = getDistance();
pc.printf("L DISTANCE: %d \n\r", distLaser);
if ((distLaser < OILRIG2_MAX)
&& (distLaser > OILRIG2_MIN)) {
fire_detected++;
} else {
fire_not_detected++;
}
}
break;
}
if (fire_detected > 0) {
return 1;
} else {
return 0;
}
}
void errFunction(void)
{
//Nothing
}
void wall_follow(int side, int direction, int section)
{
float location, set=6;
int dir=1;
pid1.reset();
if(direction == BACKWARD) dir=-1;
if(section == TOOLS)set= 10;
leftEncoder.reset();
rightEncoder.reset();
location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
while(location< 66.5) {
location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
pid1.setInputLimits(0, set);
pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
pid1.setSetPoint(set);
if(side) {
rangeFinderLeft.startMeas();
wait_ms(20);
rangeFinderLeft.getMeas(range);
} else {
rangeFinderRight.startMeas();
wait_ms(20);
rangeFinderRight.getMeas(range);
pc.printf("%d\r\n",range);
}
if(range > 15) {
//pc.printf("wavegap %f\r\n",wavegap);
// AT WAVE OPENING!!!!
motors.setMotor1Speed(dir*0.25*127);//left
motors.setMotor0Speed(dir*0.25*127);//right
} else {
pid1.setProcessValue(range);
pid_return = pid1.compute();
if(pid_return > 0) {
if(side) {
motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
motors.setMotor1Speed(dir*MAX_SPEED);//left
} else {
motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
motors.setMotor0Speed(dir*MAX_SPEED);//right
}
} else if(pid_return < 0) {
if(side) {
motors.setMotor0Speed(dir*MAX_SPEED);//right
motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
} else {
motors.setMotor1Speed(dir*MAX_SPEED);//left
motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
}
} else {
motors.setMotor0Speed(dir*MAX_SPEED);//right
motors.setMotor1Speed(dir*MAX_SPEED);//left
}
}
}
//STOP
motors.setMotor0Speed(dir*-0.3*127); //right
motors.setMotor1Speed(dir*-0.3*127); //left
wait_ms(10);
motors.stopBothMotors(0);
}
/* MODIFIED WALL_FOLLOW FOR NAVIGATION */
void wall_follow2(int side, int direction, int section, float location, int rig)
{
int dir=1, limit=86, lowlim=5;
float set=7, loc=0, Rigloc=0;
bool SeeWaveGap = false;
if(rig == 1) Rigloc= 16;
else if(rig == 2) Rigloc= 45;
else if(rig== 3) Rigloc = 70;
pid1.reset();
if(direction == BACKWARD) {
dir=-1;
limit = 100;
}
if(section == TOOLS) {
set= 6;
limit = 86;
}
if(section == RETURN) lowlim=15;
leftEncoder.reset();
rightEncoder.reset();
//pc.printf("before %f\r\n", location);
pc.printf("dir*loc+location %f\r\n",dir*loc + location );
pc.printf("limit %d \r\n", limit);
while((dir*loc + location <= limit) && (dir*loc + location >= lowlim)) {
loc=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
pc.printf("loc %f \r\n", loc);
pid1.setInputLimits(0.0, set);
pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
pid1.setSetPoint(set);
if(side) {
rangeFinderLeft.startMeas();
wait_ms(20);
rangeFinderLeft.getMeas(range);
} else {
rangeFinderRight.startMeas();
wait_ms(20);
rangeFinderRight.getMeas(range);
}
if(section == RIGS) {
rangeFinderLeft.startMeas();
wait_ms(20);
rangeFinderLeft.getMeas(range2);
if(range2< 20) {
if( abs(dir*loc + location - Rigloc) < 10) {
//STOP
motors.stopBothMotors(127);
break;
}
}
}
//pc.printf("wall follow 2 range %f\r\n",range);
//pc.printf("loc+location = %f\r\n", loc+location);
if(range > 20 ) {
if(section == RIGS || section == RETURN) {
motors.setMotor0Speed(dir*0.25*127); //right
motors.setMotor1Speed(dir*0.25*127); //left
} else {
if(!SeeWaveGap) {
SeeWaveGap=true;
} else {
//STOP
motors.stopBothMotors(127);
pc.printf("wavegap\r\n");
// AT WAVE OPENING!!!!
break;
}
}
} else {
SeeWaveGap = false;
pid1.setProcessValue(range);
pid_return = pid1.compute();
//pc.printf("Range: %f\n PID: %f\r\n", range, pid_return);
if(pid_return > 0) {
if(side) {
motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
motors.setMotor1Speed(dir*MAX_SPEED);//left
} else {
motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
motors.setMotor0Speed(dir*MAX_SPEED);//right
}
} else if(pid_return < 0) {
if(side) {
motors.setMotor0Speed(dir*MAX_SPEED);//right
motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
} else {
motors.setMotor1Speed(dir*MAX_SPEED);//left
motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
}
} else {
motors.setMotor0Speed(dir*MAX_SPEED);
motors.setMotor1Speed(dir*MAX_SPEED);
}
}
}
//STOP
motors.stopBothMotors(127);
}
void alignWithWall(int section)
{
float usValue = 0;
if(section == TOOLS) {
pc.printf("tools section align\r\n");
// turn at an angle
leftEncoder.reset();
rightEncoder.reset();
motors.setMotor0Speed(-1.2*MAX_SPEED); //right
motors.setMotor1Speed(0.4*MAX_SPEED); //left
while(rightEncoder.getPulses()>-1000);
motors.stopBothMotors(0);
//go backwards toward wall
leftEncoder.reset();
rightEncoder.reset();
motors.setMotor0Speed(-0.25*127); //right
motors.setMotor1Speed(-0.25*127); //left
while(abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300);
motors.stopBothMotors(0);
// turn left towards wall
leftEncoder.reset();
rightEncoder.reset();
motors.setMotor0Speed(MAX_SPEED); //right
motors.setMotor1Speed(-MAX_SPEED); //left
while(rightEncoder.getPulses() < 20 || abs(leftEncoder.getPulses()) < 20);
motors.stopBothMotors(0);
// turning left
motors.setMotor0Speed(0.9*MAX_SPEED); //right
motors.setMotor1Speed(-0.9*MAX_SPEED); //left
} else if(section == RIGS) {
// check distance to wall
rangeFinderRight.startMeas();
wait_ms(20);
rangeFinderRight.getMeas(range);
if(range < 3 || range > 20) return;
// turn at an angle
leftEncoder.reset();
rightEncoder.reset();
motors.setMotor1Speed(-1.2*MAX_SPEED); //left
motors.setMotor0Speed(0.4*MAX_SPEED); //right
while(abs(leftEncoder.getPulses())<500);
motors.stopBothMotors(0);
wait(2);
//go backwards toward wall
leftEncoder.reset();
rightEncoder.reset();
motors.setMotor0Speed(-0.25*127); //right
motors.setMotor1Speed(-0.25*127); //left
while(abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses()) < 200);
motors.stopBothMotors(0);
wait(2);
// turn left towards wall
leftEncoder.reset();
rightEncoder.reset();
motors.setMotor0Speed(-MAX_SPEED); //right
motors.setMotor1Speed(MAX_SPEED); //left
while(abs(rightEncoder.getPulses()) < 20 || abs(leftEncoder.getPulses()) < 20);
motors.stopBothMotors(0);
wait(2);
// turning left
motors.setMotor0Speed(-0.9*MAX_SPEED); //right
motors.setMotor1Speed(0.9*MAX_SPEED); //left
} else {// MID
pc.printf("in mid section align\r\n");
// turn right towards wall
rightTurn();
// turning left towards wall
motors.setMotor0Speed(0.9*MAX_SPEED); //right
motors.setMotor1Speed(-0.9*MAX_SPEED); //left
}
usValue = 0;
while(1) {
if(section == RIGS) {
rangeFinderRight.startMeas();
wait_ms(20);
rangeFinderRight.getMeas(range);
} else {
rangeFinderLeft.startMeas();
wait_ms(20);
rangeFinderLeft.getMeas(range);
}
pc.printf("Range %f \t OldValue %f\n\r",range, usValue);
if(range > usValue && usValue != 0 && range < 25) {
break;
} else {
usValue = range;
}
}
motors.stopBothMotors(0);
}
void rightTurn(void)
{
motors.begin();
leftEncoder.reset();
rightEncoder.reset();
motors.setMotor0Speed(-0.5*127);//right
motors.setMotor1Speed(0.5*127);//left
while(abs(leftEncoder.getPulses())<1050 || abs(rightEncoder.getPulses())<1050);
motors.stopBothMotors(127);
}
void leftTurn(void)
{
motors.begin();
leftEncoder.reset();
rightEncoder.reset();
motors.setMotor0Speed(0.5*127);// right
motors.setMotor1Speed(-0.5*127);// left
while(abs(leftEncoder.getPulses())<1100 || rightEncoder.getPulses()<1100);
motors.stopBothMotors(127);
}
void slightleft(void)
{
leftEncoder.reset();
rightEncoder.reset();
motors.setMotor0Speed(0.5*127);// right
motors.setMotor1Speed(-0.5*127);// left
while(abs(leftEncoder.getPulses())<90 || rightEncoder.getPulses()<90);
motors.stopBothMotors(127);
}
void slightright(void)
{
leftEncoder.reset();
rightEncoder.reset();
motors.setMotor0Speed(-0.4*127);// right
motors.setMotor1Speed(0.4*127);// left
while(abs(leftEncoder.getPulses())<50 || abs(rightEncoder.getPulses())<50);
motors.stopBothMotors(127);
}
void slightMove(int direction, float pulses)
{
int dir=1;
if(direction == BACKWARD) dir= -1;
leftEncoder.reset();
rightEncoder.reset();
motors.setMotor0Speed(dir*0.25*127); //right
motors.setMotor1Speed(dir*0.25*127); //left
while(abs(leftEncoder.getPulses()) < pulses || abs(rightEncoder.getPulses()) < pulses);
motors.stopBothMotors(127);
}
void UntilWall(int dir)
{
if(dir == BACKWARD) dir=-1;
leftEncoder.reset();
rightEncoder.reset();
motors.setMotor0Speed(dir*0.2*127); //right
motors.setMotor1Speed(dir*0.2*127); //left
range = 30;
while(range > 20) {
rangeFinderRight.startMeas();
wait_ms(20);
rangeFinderRight.getMeas(range);
}
motors.stopBothMotors(127);
}
void overBump(int section)
{
int preLeft=5000, preRight=5000, out=0;
motors.begin();
// slight backwards
leftEncoder.reset();
rightEncoder.reset();
motors.setMotor0Speed(-0.25*127); //right
motors.setMotor1Speed(-0.25*127); //left
while(abs(leftEncoder.getPulses()) < 50 || abs(rightEncoder.getPulses()) < 50);
motors.stopBothMotors(127);
pc.printf("slight backwards\r\n");
wait_ms(200);
// Over bump
leftEncoder.reset();
rightEncoder.reset();
motors.setMotor0Speed(0.3*127); //right
motors.setMotor1Speed(0.3*127); //left
while((abs(leftEncoder.getPulses()) < 800 || abs(rightEncoder.getPulses()) < 800) /*&& preLeft!=0*/ && IR.getIRDistance() >15 ) {
/*
preLeft=leftEncoder.getPulses();
preRight=rightEncoder.getPulses();
wait_ms(200);
if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
*/
}
pc.printf("forward \r\n");
wait_ms(200);
motors.stopBothMotors(0);
motors.begin();
preLeft=preRight=5000 ;
leftEncoder.reset();
rightEncoder.reset();
motors.setMotor0Speed(.25*127); //right
motors.setMotor1Speed(.25*127); //left
if(section == TOOLS) {
while(IR.getIRDistance() > 10 && (abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses()) < 200) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) {
if(IR.getIRDistance() > 38) break;
preLeft=leftEncoder.getPulses();
preRight=rightEncoder.getPulses();
wait_ms(200);
}
} else if(section == MID || section == MID2) {
if(section == MID2) while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 400 || abs(rightEncoder.getPulses()) < 400));
while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) {
if(IR.getIRDistance() > 38) break;
preLeft=leftEncoder.getPulses();
preRight=rightEncoder.getPulses();
wait_ms(200);
}
} else {// RIGS
while(abs(leftEncoder.getPulses()) < 220 || abs(rightEncoder.getPulses()) < 220);
// go backwards to line up with bump
leftEncoder.reset();
rightEncoder.reset();
motors.setMotor0Speed(-.15*127); //right
motors.setMotor1Speed(-.15*127); //left
while((abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100) && preLeft!=0 ) {
preLeft = leftEncoder.getPulses();
preRight = rightEncoder.getPulses();
wait_ms(200);
if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
}
motors.stopBothMotors(127);
return;
}
motors.stopBothMotors(127);
wait_ms(20);
motors.begin();
}
void to_tools_section1(float* location, float ¤t)
{
slightMove(FORWARD,6700);
current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
}
void to_tools_section2(float* location, float ¤t)
{
slightMove(FORWARD,3200);
current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
}
void from_tools_section(float* location, float ¤t)
{
alignWithWall(TOOLS);
pc.printf("align\r\n");
wait_ms(100);
//wall_follow2(LEFT,FORWARD,MID, current);
//current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
rangeFinderLeft.startMeas();
wait_ms(20);
rangeFinderLeft.getMeas(range);
if(range < 20) {
wall_follow2(LEFT,BACKWARD,TOOLS, current,0);
pc.printf("wall follow\r\n");
location[0]= current - ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
current= location[0];
pc.printf("current %f \r\n",current);
// go backwards
leftEncoder.reset();
rightEncoder.reset();
motors.setMotor0Speed(-MAX_SPEED); //right
motors.setMotor1Speed(-MAX_SPEED); //left
while(abs(leftEncoder.getPulses()) < 120 || abs(rightEncoder.getPulses())< 120);
// hard stop
motors.stopBothMotors(127);
wait_ms(100);
leftTurn();
overBump(TOOLS);
} else {
pc.printf("else greater than 20\r\n");
location[0]= current;
leftTurn();
overBump(TOOLS);
}
pc.printf("First Wavegap = %f\r\n",location[0]);
}
void tools_section(float* location, float ¤t)
{
wall_follow(LEFT,FORWARD, TOOLS);
// current position in reference to the starting position
current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
//////////////////////////////// determine tool
wait(2);
///////////////////////////////////////////////////////////////////////////////////////
// Move Forward
slightMove(FORWARD, 100);
//////////////////////////////////////////Tool aquiring
wait(2);
//////////////////////////////////////////////////////////////////// After tool is aquired
alignWithWall(TOOLS);
pc.printf("align\r\n");
wait_ms(100);
//wall_follow2(LEFT,FORWARD,MID, current);
//current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
rangeFinderLeft.startMeas();
wait_ms(20);
rangeFinderLeft.getMeas(range);
if(range < 20) {
wall_follow2(LEFT,BACKWARD,TOOLS, current,0);
pc.printf("wall follow\r\n");
location[0]= current - ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
current= location[0];
pc.printf("current %f \r\n",current);
// go backwards
leftEncoder.reset();
rightEncoder.reset();
motors.setMotor0Speed(-MAX_SPEED); //right
motors.setMotor1Speed(-MAX_SPEED); //left
while(abs(leftEncoder.getPulses()) < 120 || abs(rightEncoder.getPulses())< 120);
// hard stop
motors.stopBothMotors(127);
wait_ms(100);
leftTurn();
overBump(TOOLS);
} else {
pc.printf("else greater than 20\r\n");
location[0]= current;
leftTurn();
overBump(TOOLS);
}
pc.printf("First Wavegap = %f\r\n",location[0]);
}
void mid_section(float* location, float ¤t, int* direction)
{
motors.begin();
if(IR.getIRDistance() > 38) {
direction[0]= STRAIGHT;
overBump(MID);
return;
}
pc.printf("before align with wall \r\n");
alignWithWall(MID);
wait_ms(100);
pc.printf("mid section current = %f\r\n",current);
wall_follow2(LEFT,FORWARD,MID, current,0);
current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
pc.printf("after wf2 current = %f\r\n",current);
wait_ms(500);
rangeFinderLeft.startMeas();
wait_ms(20);
rangeFinderLeft.getMeas(range);
if(range > 20 ) {
direction[0]= RIGHT;
location[1]= current;
slightMove(FORWARD,75);
//current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
} else {
direction[0]= LEFT;
wall_follow2(LEFT,BACKWARD,MID,current,0);
location[1]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
current= location[1];
if(location[1] < 18) {
slightMove(FORWARD, 50);
current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
}
}
pc.printf("wavegap2 = %f\r\n",location[1]);
leftTurn();
wait_ms(100);
overBump(MID);
}
void mid_section2(float* location, float ¤t, int* direction)
{
motors.begin();
pc.printf("mid section 2\r\n");
if(IR.getIRDistance() > 38) {
direction[1]= STRAIGHT;
overBump(RIGS);
return;
}
alignWithWall(MID);
pc.printf("midsection 2 alignt with wall mid \r\n");
wall_follow2(LEFT,FORWARD,MID, current,0);
current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
wait_ms(500);
pc.printf("midseection 2 after wf2 %f",current);
rangeFinderLeft.startMeas();
wait_ms(20);
rangeFinderLeft.getMeas(range);
if(range > 20 ) {
direction[1]= RIGHT;
location[2]= current;
slightMove(FORWARD,100);
current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
} else {
direction[1]= LEFT;
wall_follow2(LEFT,BACKWARD,MID,current,0);
location[2]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
current=location[2];
//slightMove(FORWARD,100);
}
leftTurn();
overBump(RIGS);
pc.printf("overbump rigs\r\n");
}
void rig_section(float* location, float ¤t, int* direction, int rig)
{
float loc;
if(rig == 1) loc= 15;
else if(rig == 2) loc= 45;
else loc = 75;
// Slight forward for turn
slightMove(FORWARD,100);
wait_ms(100);
rightTurn();
slightright();
wait(5);
if(current > loc) {
pc.printf("RIG section %f\r\n",current);
wall_follow2(RIGHT, BACKWARD, RIGS, current, rig);
current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
} else {
pc.printf("RIG section %f\r\n",current);
wall_follow2(RIGHT, FORWARD, RIGS, current, rig);
current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
}
alignWithWall(RIGS);
}
void tools_section_return(float* location, float ¤t)
{
if(location[0] > 16) {
leftTurn();
wall_follow2(LEFT, BACKWARD, RETURN, location[0], 0);
}
motors.stopBothMotors(0);
}
void mid_section_return(float* location, float ¤t, int* direction)
{
if(direction[0] == RIGHT) {
leftTurn();
alignWithWall(MID);
wall_follow2(LEFT, BACKWARD, MID, current,0);
current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
rightTurn();
} else if(direction[0] == LEFT) {
leftTurn();
wall_follow2(RIGHT, FORWARD, MID, current,0);
current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
rightTurn();
}
//ELSE and GO FORWARD
overBump(RIGS);
}
void mid_section2_return(float* location, float ¤t, int* direction)
{
if(direction[1] == RIGHT) {
leftTurn();
wall_follow2(LEFT, BACKWARD, MID, current,0);
current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
rightTurn();
} else if(direction[1] == LEFT) {
leftTurn();
wall_follow2(RIGHT, FORWARD, MID, current,0);
current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
rightTurn();
}
//ELSE and GO FORWARD
overBump(MID);
}
void rig_section_return(float* location, float ¤t, int* direction)
{
if(location[2] > current) {
wall_follow2(RIGHT, FORWARD, MID, current,0);
current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
} else {
wall_follow2(RIGHT, BACKWARD, MID, current,0);
current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
}
rightTurn();
overBump(MID2);
}