UH Robotics
uyvug
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of
theRobot
by 
Thomas Ashworth
Diff: main.cpp
- Revision:
- 28:2bb6b0fe39d0
- Parent:
- 27:5540aa3c828c
- Child:
- 29:22b243e288c8
diff -r 5540aa3c828c -r 2bb6b0fe39d0 main.cpp
--- a/main.cpp Tue Apr 15 17:13:35 2014 +0000
+++ b/main.cpp Wed Apr 16 20:45:30 2014 +0000
@@ -9,12 +9,12 @@
#include "stdio.h"
#include "LPC17xx.h"
#include "Sharp.h"
-
-
+
+
#define PI 3.14159
-
-
-
+
+
+
/* Navigation Definitions */
#define PIN_TRIGGERL (p12)
#define PIN_ECHOL (p11)
@@ -38,8 +38,8 @@
#define MID2 (3)
#define RETURN (4)
#define FAR (1)
-
-
+
+
//States
#define START 0
#define OILRIG1_POS 1
@@ -60,9 +60,9 @@
#define END 16
#define GOTO_TOOLS2 17
#define RETURN_TO_START 18
-
-
-
+
+
+
//Servo Static Positions
#define STORE_POSITION 0
#define OIL_RIG1 1
@@ -74,19 +74,15 @@
#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
-
+#define INSERT_TOOL 10
+
+
//Rig definitions
#define SQUARE 1
#define TRIANGLE 2
#define CIRCLE 3
-
-
+
+
//Oil Rig distance thresholds
#define OILRIG1_MAX 1800
#define OILRIG1_MIN 1000
@@ -94,25 +90,25 @@
#define OILRIG2_MIN 1000
#define OILRIG3_MAX 1800
#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
extern Serial lrf; //Laser Range Finder lrf(p13,p14)
@@ -127,10 +123,10 @@
Sharp IRFront(p19);
Sharp IRBack(p20);
//InterruptIn encoder(p29);
-
-
-
-
+
+
+
+
/***************
local servo functions
****************/
@@ -142,8 +138,8 @@
void servoPosition(int set);
int fire_checker(int rig);
int button_start = 0;
-
-
+
+
//Navigation Functions
void wall_follow2(int side, int direction, int section, float location, int rig);
void leftTurn(void);
@@ -164,13 +160,13 @@
void overBump(int section);
void alignWithWall(int section);
void UntilWall(int dir);
-
-void testSensors(void);
+
+void testSensors(void);
float normd(int* pop, int count, int threshold);
int Xadjust(int tool);
-
+
extern "C" void mbed_reset();
-
+
/************
Main Variables
*************/
@@ -180,14 +176,14 @@
int shape_detected = 0;
float range, range2, range3, pid_return;
int num, input;
-
-
+
+
/************
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;
@@ -197,37 +193,33 @@
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, 164, 20, 60, 44, 175, 0}, // point laser at oilrig1
- {OIL_RIG2, 164, 20, 60, 44, 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, 101, 50, 80, 113, 90, 0}, // Look over first tool
- {TOOL_2, 82, 50, 80, 113, 90, 0}, // Look over second tool
- {TOOL_3, 62, 50, 80, 112, 90, 0}, // Look over third tool
- {DRIVE_POSITION_TOOL, 85, 10, 0, 165, 90, 105}, // Drive with tool loaded
+
+ {STORE_POSITION, 85, 5, 0, 170, 60, 0}, // storing position
+ {OIL_RIG1, 164, 20, 60, 100, 175, 0}, // point laser at oilrig1
+ {OIL_RIG2, 164, 20, 60, 100, 175, 0}, // point laser at oilrig2
+ {OIL_RIG3, 130, 90, 90, 100, 175, 0}, // NOT USED!!!!! point laser at oilrig2
+ {DRIVE_POSITION_NOTOOL, 85, 10, 0, 165, 60, 0}, // Drive through course
+ {TOOL_1, 101, 50, 80, 133, 60, 0}, // Look over first tool
+ {TOOL_2, 82, 50, 80, 133, 60, 0}, // Look over second tool
+ {TOOL_3, 62, 50, 80, 132, 60, 0}, // Look over third tool
+ {DRIVE_POSITION_TOOL, 85, 10, 0, 165, 140, 120}, // Drive with tool loaded
{ORIENT_TOOL, 135, 60, 75, 60, 90, 90}, // position tool to be inserted
- {PU_TOOL_1, 99, 46, 78, 110, 170, 0}, // STATIC Pickup tool POSITION
- {PU_TOOL_2, 76, 47, 80, 112, 170, 0}, // STATIC Pickup tool POSITION
- {PU_TOOL_3, 59, 44, 76, 110, 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
+ {INSERT_TOOL_1, 170, 50, 127, 52, 140, 120}, // InsertToolIntoRig
+
};
-
-
+
+
/* Variables for imageprocessing and distance */
int x_coord;
int y_coord;
@@ -236,13 +228,13 @@
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) {
@@ -254,50 +246,66 @@
}
return;
}
-
-
-
+
+
+
int main()
{
-
-
+
+
/*****************
INITIALIZATIONS
*******************/
float location[3], current=4;
int direction[3];
double distance;
-
+
int pu, num, input;
-
-
+
+
pc.baud(115200);
-
+
//testSensors();
-
+
//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(1) {
if(button_start == 1) {
-
-
+
+ /*
+ pc.printf("Servo Test");
+ while(1) {
+ pc.scanf("%d %d", &servoNum, &servoAngle);
+ if(servoAngle > 175) {
+ servoAngle = 175;
+ }
+ if(servoNum > 5 ) {
+ servoNum = 0;
+ servoAngle = 90;
+ }
+ setServoPulse(servoNum, servoAngle);
+
+ };*/
+
+
switch (state) {
-
+
/**************************************************
* STAGE 0
*
@@ -306,27 +314,27 @@
**************************************************/
case START :
myled1 = 1;
-
+
//current=75;
//state = NAVIGATE_WAVES_ROW1;
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");
@@ -337,9 +345,9 @@
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);
@@ -347,12 +355,12 @@
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!!!!!!!!");
@@ -364,7 +372,7 @@
state = GOTO_TOOLS2;
}
break;
-
+
/**************************************************
* STAGE 2
*
@@ -379,13 +387,13 @@
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);
@@ -393,15 +401,15 @@
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,3250);
current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
-
+
state = IDENTIFY_TOOLS;
break;
-
-
-
+
+
+
/**************************************************
* STAGE 3
*
@@ -410,11 +418,11 @@
*
**************************************************/
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);
@@ -424,28 +432,28 @@
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;
}
}*/
-
+
// aveArea = sumArea/count;
//printImageToFile(BINARY);
//either goes to aquire the tool or look at the next shape
-
+
//****************//if(Xadjust(TOOL_2) == tool_needed) {
if(Xadjust(TOOL_2) == 162) {
//printImageToFile(BINARY);
@@ -463,19 +471,19 @@
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;
}
@@ -490,11 +498,11 @@
state = AQUIRE_TOOL3;
}
}
-
+
break;
-
+
case AQUIRE_TOOL1:
-
+
servoPosition(PU_TOOL_1);
setServoPulse(4, 175);
wait(5);
@@ -502,29 +510,31 @@
wait(1);
setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm - 5);
wait(1);
+ setServoPulse(4, 175);
+ wait(2);
setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm - 6);
wait(.5);
- setServoPulse(5, 140);
+ setServoPulse(5, 100);
wait(.5);
setServoPulse(5, 00);
setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm - 10);
wait(1);
- setServoPulse(5, 140);
+ setServoPulse(5, 120);
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);
@@ -535,59 +545,61 @@
wait(1);
setServoPulse(3, Arm_Table[PU_TOOL_2].claw_arm - 2);
wait(1);
+ setServoPulse(4, 175);
+ wait(2);
setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 6);
wait(2);
- setServoPulse(5, 140);
+ setServoPulse(5, 100);
wait(2);
setServoPulse(5, 00);
setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 10);
wait(2);
- setServoPulse(5, 140);
+ setServoPulse(5, 120);
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;
}
}
-
+
*/
Xadjust(TOOL_3);
-
+
setServoPulse(4, 175);
wait(5);
setServoPulse(0, Arm_Table[PU_TOOL_3].base_rotate - 1);
@@ -596,50 +608,52 @@
wait(1);
setServoPulse(3, Arm_Table[PU_TOOL_2].claw_arm - 3);
wait(1);
+ setServoPulse(4, 175);
+ wait(2);
setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm - 9);
wait(.5);
- setServoPulse(5, 110);
+ setServoPulse(5, 100);
wait(.5);
setServoPulse(5, 00);
setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm - 10);
wait(1);
- setServoPulse(5, 115);
+ setServoPulse(5, 120);
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:
//shape_detected = 1;
if(tool_needed == 1) {
@@ -654,10 +668,10 @@
rig_section(location, current, direction, 3);
//state = NAVIGATE_TO_CIRCLE_RIG;
state = RETURN_TO_START;
-
+
}
break;
-
+
/**************************************************
* STAGE 5
*
@@ -676,7 +690,7 @@
//NAVIGATION CODE HERE
state = RIG_ALIGN;
break;
-
+
/**************************************************
* STAGE 6
*
@@ -684,17 +698,17 @@
*
**************************************************/
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
*
@@ -703,14 +717,14 @@
* - win contest
*
**************************************************/
-
+
case INSERT_TOOL:
//*********************//
//******* TODO ********//
//*********************//
// CODE TO INSERT TOOL
break;
-
+
/**************************************************
* STAGE 8
*
@@ -718,7 +732,7 @@
*
**************************************************/
case RETURN_TO_START:
- wait(3);
+ wait(3);
rig_section_return(location, current, direction);
mid_section2_return(location, current, direction);
mid_section_return(location, current, direction);
@@ -726,7 +740,7 @@
while(1);
state = END;
break;
-
+
/**************************************************
* STAGE 9
*
@@ -745,24 +759,24 @@
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;
@@ -787,16 +801,16 @@
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");
@@ -808,7 +822,7 @@
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);
@@ -817,7 +831,7 @@
//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)
{
@@ -826,9 +840,9 @@
pulse = 4094 * pulse / pulselength;
pwm.setPWM(n, 0, pulse);
}
-
-
-
+
+
+
void servoPosition(int set)
{
//moves to current position
@@ -843,7 +857,7 @@
int fire_checker(int rig)
{
switch (rig) {
-
+
case 1:
for (int i = 0; i<10; i++) {
distLaser = getDistance();
@@ -868,71 +882,71 @@
}
}
break;
-
+
}
-
+
if (fire_detected > 0) {
return 1;
} else {
return 0;
}
}
-
+
void errFunction(void)
{
pc.printf("\n\nERROR: %d", motors.getError() );
-
+
}
-
-
+
+
/* MODIFIED WALL_FOLLOW FOR NAVIGATION */
-
+
void wall_follow2(int side, int direction, int section, float location, int rig)
{
int dir=1, limit=80, lowlim=4;
float set=28, loc=0, Rigloc=0, location_cal=0;
bool SeeWaveGap = false;
-
+
if(rig == 1) Rigloc= 16;
else if(rig == 2) Rigloc= 45;
else if(rig== 3) Rigloc = 70;
-
+
pid1.reset();
-
+
if(section == TOOLS) {
limit = 100;
lowlim=10;
- }else if(section == RIGS) set = 18;
+ } else if(section == RIGS) set = 18;
else if(section == MID2) limit =85;
-
+
if(direction == BACKWARD) {
dir=-1;
limit = 100;
} else if(direction == FORWARD) lowlim=-20;
-
+
if(location <4) limit=80;
-
+
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);
if((section == RIGS || section == RETURN) && side == LEFT)location_cal= -1*dir*loc + location;
else location_cal= dir*loc + location;
-
+
while((location_cal <= limit) && (location_cal >= lowlim)) {
-
+
loc=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
-
+
if((section == RIGS || section == RETURN) && side == LEFT)location_cal= -1*dir*loc + location;
else location_cal= dir*loc + location;
-
+
pid1.setInputLimits(0.0, set);
pid1.setOutputLimits( -MAX_SPEED1, MAX_SPEED1);
pid1.setSetPoint(set);
-
+
if(side) {
range = IRFront.getIRDistance();
range3= IRBack.getIRDistance();
@@ -941,18 +955,18 @@
wait_ms(20);
rangeFinderRight.getMeas(range);
}
-
+
if(section == RIGS) {
- if(side == LEFT){
+ if(side == LEFT) {
range2 = IRFront.getIRDistance();
range3= IRBack.getIRDistance();
- }else{
+ } else {
rangeFinderRight.startMeas();
wait_ms(20);
rangeFinderRight.getMeas(range2);
}
-
+
if(range2< 15) {
if( abs(location_cal - Rigloc) < 10) {
//STOP
@@ -961,8 +975,8 @@
}
}
}
-
-
+
+
//pc.printf("wall follow 2 range %f\r\n",range);
//pc.printf("loc+location = %f\r\n", loc+location);
if(range > 35 && range3 > 35) {
@@ -972,13 +986,13 @@
motors.stopBothMotors(127);
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
@@ -1001,16 +1015,16 @@
}
}
}
-
+
//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
@@ -1020,7 +1034,7 @@
motors.setMotor1Speed(0.4*MAX_SPEED); //left
while(rightEncoder.getPulses()>-1000);
motors.stopBothMotors(0);
-
+
//go backwards toward wall
leftEncoder.reset();
rightEncoder.reset();
@@ -1028,14 +1042,14 @@
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(127);
wait_ms(300);
return;
@@ -1043,7 +1057,7 @@
rangeFinderRight.startMeas();
wait_ms(20);
rangeFinderRight.getMeas(range);
-
+
if(range>15){
// turning left
leftEncoder.reset();
@@ -1058,9 +1072,9 @@
// turning left
//motors.setMotor0Speed(0.9*MAX_SPEED); //right
//motors.setMotor1Speed(-0.9*MAX_SPEED); //left
-
+
} else if(section == RIGS) {
-
+
// turn left at an angle
leftEncoder.reset();
rightEncoder.reset();
@@ -1069,7 +1083,7 @@
while(abs(leftEncoder.getPulses())<500);
motors.stopBothMotors(0);
wait_ms(200);
-
+
//go backwards away form wall
leftEncoder.reset();
rightEncoder.reset();
@@ -1078,14 +1092,14 @@
while(abs(leftEncoder.getPulses()) < 150 || abs(rightEncoder.getPulses()) < 150);
motors.stopBothMotors(127);
wait(2);
-
+
// turn right away from wall
leftEncoder.reset();
rightEncoder.reset();
motors.setMotor0Speed(-MAX_SPEED); //right
motors.setMotor1Speed(MAX_SPEED); //left
while(abs(rightEncoder.getPulses()) < 25 || abs(leftEncoder.getPulses()) < 25 );
-
+
motors.stopBothMotors(127);
} else if(section == MID2) {
@@ -1119,18 +1133,17 @@
slightMove(FORWARD,100);
}
return;
-
- }
- else{ // MID
+
+ } 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
-
+ //motors.setMotor1Speed(-0.9*MAX_SPEED); //left
+
}
-
+
usValue = 0;
/* while(1) {
if(section == 10) { // CURENTLY NOT USED (WAS RIGS)
@@ -1149,7 +1162,7 @@
}
motors.stopBothMotors(0);*/
}
-
+
void rightTurn(void)
{
motors.begin();
@@ -1160,38 +1173,38 @@
while(abs(leftEncoder.getPulses())<800 || abs(rightEncoder.getPulses())<800);
motors.stopBothMotors(127);
wait_ms(300);
-
+
if(IRFront.getIRDistance() > 35 && IRBack.getIRDistance() > 35) return;
// align with wave
- while(IRFront.getIRDistance() > 35){
+ while(IRFront.getIRDistance() > 35) {
pc.printf("front sensor sees gap\r\n");
motors.setMotor0Speed(-0.1*127);//right
motors.setMotor1Speed(-0.1*127);//left
}
- while(IRBack.getIRDistance() >35 ){
- pc.printf("back sensor sees gap\r\n");
+ while(IRBack.getIRDistance() >35 ) {
+ pc.printf("back sensor sees gap\r\n");
motors.setMotor0Speed(0.1*127);//right
- motors.setMotor1Speed(0.1*127);//left
+ motors.setMotor1Speed(0.1*127);//left
}
motors.stopBothMotors(127);
wait_ms(300);
-
- while(IRFront.getIRDistance() - IRBack.getIRDistance() > 0.5){
- pc.printf("turn left\r\n");
- motors.setMotor0Speed(0.3*127);// right
- motors.setMotor1Speed(-0.3*127);// left
+
+ while(IRFront.getIRDistance() - IRBack.getIRDistance() > 0.5) {
+ pc.printf("turn left\r\n");
+ motors.setMotor0Speed(0.3*127);// right
+ motors.setMotor1Speed(-0.3*127);// left
}
- while(IRFront.getIRDistance() - IRBack.getIRDistance() < -0.5){
- pc.printf("turn right\r\n");
- motors.setMotor0Speed(-0.3*127);//right
- motors.setMotor1Speed(0.3*127);// left
+ while(IRFront.getIRDistance() - IRBack.getIRDistance() < -0.5) {
+ pc.printf("turn right\r\n");
+ motors.setMotor0Speed(-0.3*127);//right
+ motors.setMotor1Speed(0.3*127);// left
}
-
+
motors.stopBothMotors(127);
wait_ms(300);
-
+
}
-
+
void leftTurn(void)
{
motors.begin();
@@ -1201,43 +1214,43 @@
motors.setMotor1Speed(-0.5*127);// left
while(abs(leftEncoder.getPulses())<1075 || rightEncoder.getPulses()<1075);
motors.stopBothMotors(127);
-
+
wait_ms(300);
-
+
if(IRFront.getIRDistance() > 35 && IRBack.getIRDistance() > 35) return;
// align with wave
- while(IRFront.getIRDistance() > 35){
+ while(IRFront.getIRDistance() > 35) {
pc.printf("front sensor sees gap\r\n");
motors.setMotor0Speed(-0.1*127);//right
motors.setMotor1Speed(-0.1*127);//left
}
- while(IRBack.getIRDistance() >35 ){
- pc.printf("back sensor sees gap\r\n");
+ while(IRBack.getIRDistance() >35 ) {
+ pc.printf("back sensor sees gap\r\n");
motors.setMotor0Speed(0.1*127);//right
- motors.setMotor1Speed(0.1*127);//left
+ motors.setMotor1Speed(0.1*127);//left
}
motors.stopBothMotors(127);
wait_ms(300);
-
- while(IRFront.getIRDistance() - IRBack.getIRDistance() > 0.5){
- pc.printf("turn left\r\n");
- motors.setMotor0Speed(0.3*127);// right
- motors.setMotor1Speed(-0.3*127);// left
+
+ while(IRFront.getIRDistance() - IRBack.getIRDistance() > 0.5) {
+ pc.printf("turn left\r\n");
+ motors.setMotor0Speed(0.3*127);// right
+ motors.setMotor1Speed(-0.3*127);// left
}
- while(IRFront.getIRDistance() - IRBack.getIRDistance() < -0.5){
- pc.printf("turn right\r\n");
- motors.setMotor0Speed(-0.3*127);//right
- motors.setMotor1Speed(0.3*127);// left
+ while(IRFront.getIRDistance() - IRBack.getIRDistance() < -0.5) {
+ pc.printf("turn right\r\n");
+ motors.setMotor0Speed(-0.3*127);//right
+ motors.setMotor1Speed(0.3*127);// left
}
-
+
motors.stopBothMotors(127);
- wait_ms(300);
-
+ wait_ms(300);
+
}
-
+
void slightleft(void)
{
-
+
leftEncoder.reset();
rightEncoder.reset();
motors.setMotor0Speed(0.5*127);// right
@@ -1245,10 +1258,10 @@
while(abs(leftEncoder.getPulses())<70 || rightEncoder.getPulses()<70);
motors.stopBothMotors(127);
}
-
+
void slightright(void)
{
-
+
leftEncoder.reset();
rightEncoder.reset();
motors.setMotor0Speed(-0.4*127);// right
@@ -1256,48 +1269,48 @@
while(abs(leftEncoder.getPulses())<200 || abs(rightEncoder.getPulses())<200);
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)
{
float avg=0;
int i;
-
+
// first set
motors.setMotor1Speed(0.1*127);//left
motors.setMotor0Speed(0.1*127);//right
@@ -1326,14 +1339,14 @@
wait_ms(500);
if(section!= RIGS) {
range = 0;
-
- do{
+
+ do {
rangeFinderFront.startMeas();
wait_ms(1);
while(rangeFinderFront.getMeas(range) != RANGE_MEAS_INVALID);
pc.printf("Ultrasonic front sensor %f\r\n", range);
- if(range < 9){
+ if(range < 9) {
motors.stopBothMotors(127);
wait_ms(200);
break;
@@ -1344,31 +1357,31 @@
wait_ms(220);
motors.stopBothMotors(127);
wait_ms(500);
-
- }while(range > 9);
-
+
+ } while(range > 9);
+
motors.setMotor1Speed(0.1*127);//left
motors.setMotor0Speed(0.1*127);//right
wait_ms(100);
while(motors.GetMotor0Current()<5 || motors.GetMotor1Current() <5);
motors.stopBothMotors(127);
- wait_ms(200);
-
+ wait_ms(200);
+
} else {
leftEncoder.reset();
rightEncoder.reset();
-
+
while(leftEncoder.getPulses()<500 || rightEncoder.getPulses()<500) {
motors.setMotor1Speed(0.1*127);//left
motors.setMotor0Speed(0.1*127);//right
wait_ms(220);
- if(motors.GetMotor0Current()>5 || motors.GetMotor1Current() >5){
+ if(motors.GetMotor0Current()>5 || motors.GetMotor1Current() >5) {
motors.setMotor1Speed(0.3*127);//left
motors.setMotor0Speed(0.3*127);//right
wait_ms(220);
motors.stopBothMotors(127);
- wait_ms(200);
+ wait_ms(200);
leftEncoder.reset();
rightEncoder.reset();
}
@@ -1378,43 +1391,43 @@
}
-
-
+
+
motors.stopBothMotors(127);
wait_ms(20);
motors.begin();
-
+
}
-
-
+
+
void to_tools_section1(float* location, float ¤t)
{
slightMove(FORWARD,6650);
current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
-
+
}
-
+
void to_tools_section2(float* location, float ¤t)
{
slightMove(FORWARD,3250);
current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
-
+
}
-
+
void from_tools_section(float* location, float ¤t)
{
-
+
pc.printf("IR front %f \t back %f \r\n", IRFront.getIRDistance(), IRBack.getIRDistance());
-
+
if(IRFront.getIRDistance() < 37 || IRBack.getIRDistance() < 37) {
wall_follow2(LEFT,BACKWARD,TOOLS, current,0);
- pc.printf("IR front %f \t back %f \r\n", IRFront.getIRDistance(), IRBack.getIRDistance());
+ pc.printf("IR front %f \t back %f \r\n", IRFront.getIRDistance(), IRBack.getIRDistance());
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
//slightMove(BACKWARD,200);
-
+
wait_ms(100);
leftTurn();
slightleft();
@@ -1427,17 +1440,17 @@
overBump(TOOLS);
}
pc.printf("First Wavegap = %f\r\n",location[0]);
-
+
}
-
+
void mid_section(float* location, float ¤t, int* direction)
-{
+{
wait_ms(500);
-
+
rangeFinderFront.startMeas();
wait_ms(20);
rangeFinderFront.getMeas(range);
-
+
if(range > 20) {
leftEncoder.reset();
rightEncoder.reset();
@@ -1452,7 +1465,7 @@
//alignWithWall(MID);
//current-=4;
//wait_ms(200);
-
+
//if(current > 20){
//alignWithWall(MID2);
//current-=4;
@@ -1463,9 +1476,9 @@
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);
-
+
if(IRFront.getIRDistance() > 37 && IRBack.getIRDistance() > 37) {
wait(1);
direction[0]= RIGHT;
@@ -1478,15 +1491,14 @@
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, 75);
current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
- }
- else slightMove(BACKWARD,75);
-
+ } else slightMove(BACKWARD,75);
+
}
-
+
wait_ms(200);
//pc.printf("wavegap2 = %f\r\n",location[1]);
//left turn
@@ -1497,21 +1509,21 @@
motors.setMotor1Speed(-0.5*127);// left
while(abs(leftEncoder.getPulses())<110 || rightEncoder.getPulses()<1100);
motors.stopBothMotors(127);
-
+
wait_ms(100);
-
+
overBump(MID);
}
-
+
void mid_section2(float* location, float ¤t, int* direction)
{
//pc.printf("mid section 2\r\n");
wait_ms(500);
-
+
rangeFinderFront.startMeas();
wait_ms(20);
rangeFinderFront.getMeas(range);
-
+
if(range > 20) {
leftEncoder.reset();
rightEncoder.reset();
@@ -1522,22 +1534,22 @@
overBump(RIGS);
return;
}
-
+
//alignWithWall(MID);
wait_ms(100);
-
+
rightTurn();
//slightright();
wait_ms(100);
-
-
+
+
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);
-
+
if(IRFront.getIRDistance() > 37 && IRBack.getIRDistance() > 37) {
direction[1]= RIGHT;
location[2]= current;
@@ -1550,7 +1562,7 @@
current=location[2];
//slightMove(BACKWARD,100);
}
-
+
//LEFT turn
motors.begin();
leftEncoder.reset();
@@ -1559,26 +1571,26 @@
motors.setMotor1Speed(-0.5*127);// left
while(abs(leftEncoder.getPulses())<1115 || rightEncoder.getPulses()<1115);
motors.stopBothMotors(127);
-
+
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,150);
wait_ms(100);
leftTurn();
//slightright();
-
-
+
+
if(current > loc) {
//pc.printf("RIG section %f\r\n",current);
wall_follow2(LEFT, FORWARD, RIGS, current, rig);
@@ -1596,7 +1608,7 @@
slightMove(FORWARD, 75);
current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
}
-
+
void tools_section_return(float* location, float ¤t)
{
if(location[0] > 16) {
@@ -1604,9 +1616,9 @@
wall_follow2(LEFT, BACKWARD, TOOLS, location[0], 0);
}
motors.stopBothMotors(127);
-
+
}
-
+
void mid_section_return(float* location, float ¤t, int* direction)
{
if(direction[0] == RIGHT) {
@@ -1627,7 +1639,7 @@
//ELSE and GO FORWARD
overBump(RIGS);
}
-
+
void mid_section2_return(float* location, float ¤t, int* direction)
{
if(direction[1] == RIGHT) {
@@ -1644,11 +1656,11 @@
//ELSE and GO FORWARD
overBump(MID);
}
-
+
void rig_section_return(float* location, float ¤t, int* direction)
{
alignWithWall(RIGS);
-
+
if(location[2] > current) {
wall_follow2(LEFT, BACKWARD, RETURN, current,0);
current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
@@ -1658,7 +1670,7 @@
wall_follow2(LEFT, FORWARD, RETURN, current,0);
current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
}
-
+
// LEFT TURN
motors.begin();
leftEncoder.reset();
@@ -1666,14 +1678,14 @@
motors.setMotor0Speed(0.5*127);// right
motors.setMotor1Speed(-0.5*127);// left
while(abs(leftEncoder.getPulses())<1100 || rightEncoder.getPulses()<110);
-
+
motors.stopBothMotors(127);
overBump(MID2);
}
-
-
-
-
+
+
+
+
int Xadjust(int tool)
{
int areaArray[10];
@@ -1691,14 +1703,14 @@
setServoPulse2(0,Arm_Table[tool].base_rotate += (deltaX/60.0) );
Arm_Table[PU_TOOL_2].base_rotate = Arm_Table[TOOL_2].base_rotate;
}
-
-
+
+
}
-
+
C = normd(areaArray, 10, CIRCLE_AREA);
// S = normd(areaArray, 10, SQUARE_AREA);
// T = normd(areaArray, 10, TRIANGLE_AREA);
-
+
if((C < SQUARE_AREA) && (C > CIRCLE_AREA)) {
pc.printf("\n\nCIRCLE DETECTED\n\r");
return CIRCLE;
@@ -1709,8 +1721,8 @@
pc.printf("\n\nTRIANGLE DETECTED\n\r");
return TRIANGLE;
}
-
-
+
+
/*
if((C < S) && (C < T)) {
pc.printf("\n\nCIRCLE DETECTED\n\r");
@@ -1723,7 +1735,7 @@
return TRIANGLE;
}*/
}
-
+
float normd(int* pop, int count, int threshold)
{
int i = 0;
@@ -1733,34 +1745,35 @@
}
mean /= (float)count;
pc.printf("\n\nMean: %f\n\r", mean);
-
+
for(i=0; i<count; i++) {
std += pow(((float)pop[i]-mean),2);
}
std /= (float)count;
std = sqrt(std);
//pc.printf("\n\nStd: %f\n\r", std);
-
+
//pc.printf("\n\nNorm: %f\n\r", (1/(std*sqrt(2*PI)))*exp(-pow(((float)threshold-mean),2)/(2*pow(std,2))));
-
-
+
+
//return abs(mean - threshold);
return mean;
//return (1/(std*sqrt(2*PI)))*exp(-pow(((float)threshold-mean),2)/(2*pow(std,2)));
-
+
}
-void testSensors(void){
+void testSensors(void)
+{
float range, range2;
-
- while(1){
+
+ while(1) {
rangeFinderFront.startMeas();
rangeFinderRight.startMeas();
wait_ms(20);
rangeFinderFront.getMeas(range);
rangeFinderRight.getMeas(range2);
-
- pc.printf("us front %f \t us right %f \t IR front %f \t IR back %f\r\n", range, range2, IRFront.getIRDistance(), IRBack.getIRDistance());
- //pc.printf("IR front %f \t IR back %f\r\n", IRFront.getIRDistance(), IRBack.getIRDistance());
+
+ pc.printf("us front %f \t us right %f \t IR front %f \t IR back %f\r\n", range, range2, IRFront.getIRDistance(), IRBack.getIRDistance());
+ //pc.printf("IR front %f \t IR back %f\r\n", IRFront.getIRDistance(), IRBack.getIRDistance());
}
}
\ No newline at end of file