Paolina Povolotskaya / NavigationTest

revise

Dependencies:   HCSR04 PID PololuQik2 QEI mbed-rtos Sharp

main.cpp@0:ff94cc47fef7, 2014-03-12 (annotated)

Committer:
Fairy_Paolina
Date:
Wed Mar 12 21:55:44 2014 +0000
Revision:
0:ff94cc47fef7
Child:
1:801f0b9a862a
revise;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Fairy_Paolina 0:ff94cc47fef7 1 #include "rtos.h"
Fairy_Paolina 0:ff94cc47fef7 2 #include "PID.h"
Fairy_Paolina 0:ff94cc47fef7 3 #include "PololuQik2.h"
Fairy_Paolina 0:ff94cc47fef7 4 #include "QEI.h"
Fairy_Paolina 0:ff94cc47fef7 5 #include "mbed.h"
Fairy_Paolina 0:ff94cc47fef7 6 #include "HCSR04.h"
Fairy_Paolina 0:ff94cc47fef7 7 #include "stdio.h"
Fairy_Paolina 0:ff94cc47fef7 8 #include "LPC17xx.h"
Fairy_Paolina 0:ff94cc47fef7 9
Fairy_Paolina 0:ff94cc47fef7 10 #define PIN_TRIGGER (p12)
Fairy_Paolina 0:ff94cc47fef7 11 #define PIN_ECHO (p11)
Fairy_Paolina 0:ff94cc47fef7 12 #define PULSE_PER_REV (1192)
Fairy_Paolina 0:ff94cc47fef7 13 #define WHEEL_CIRCUM (12.56637)
Fairy_Paolina 0:ff94cc47fef7 14 #define DIST_PER_PULSE (0.01054225722682)
Fairy_Paolina 0:ff94cc47fef7 15 #define MTRS_TO_INCH (39.3701)
Fairy_Paolina 0:ff94cc47fef7 16 #define MAX_SPEED (0.2*127)
Fairy_Paolina 0:ff94cc47fef7 17
Fairy_Paolina 0:ff94cc47fef7 18 float range, pid_return;
Fairy_Paolina 0:ff94cc47fef7 19 void errFunction(void);
Fairy_Paolina 0:ff94cc47fef7 20 bool cRc;
Fairy_Paolina 0:ff94cc47fef7 21
Fairy_Paolina 0:ff94cc47fef7 22 //Hardware Initialization
Fairy_Paolina 0:ff94cc47fef7 23 Serial bt(p13,p14);
Fairy_Paolina 0:ff94cc47fef7 24 Serial pc(USBTX,USBRX);
Fairy_Paolina 0:ff94cc47fef7 25 HCSR04 rangeFinder( PIN_TRIGGER, PIN_ECHO );
Fairy_Paolina 0:ff94cc47fef7 26 PID pid1(1.0,0.0,0.0,0.1);
Fairy_Paolina 0:ff94cc47fef7 27 PololuQik2 motors(p9, p10, p8, p15, errFunction, cRc);
Fairy_Paolina 0:ff94cc47fef7 28
Fairy_Paolina 0:ff94cc47fef7 29 //Functions
Fairy_Paolina 0:ff94cc47fef7 30
Fairy_Paolina 0:ff94cc47fef7 31 void wall_follow(void);
Fairy_Paolina 0:ff94cc47fef7 32 void wall_follow2(int &currentLocation);
Fairy_Paolina 0:ff94cc47fef7 33 void wall_follow3(int &currentLocation, int &WaveOpening);
Fairy_Paolina 0:ff94cc47fef7 34
Fairy_Paolina 0:ff94cc47fef7 35 void us_distance(void);
Fairy_Paolina 0:ff94cc47fef7 36
Fairy_Paolina 0:ff94cc47fef7 37
Fairy_Paolina 0:ff94cc47fef7 38 int main(void){
Fairy_Paolina 0:ff94cc47fef7 39
Fairy_Paolina 0:ff94cc47fef7 40 pc.baud(115200);
Fairy_Paolina 0:ff94cc47fef7 41 motors.begin();
Fairy_Paolina 0:ff94cc47fef7 42
Fairy_Paolina 0:ff94cc47fef7 43 //motors.setMotor0Speed(MAX_SPEED); //left
Fairy_Paolina 0:ff94cc47fef7 44 //motors.setMotor1Speed(MAX_SPEED); //right
Fairy_Paolina 0:ff94cc47fef7 45 //wait(10);
Fairy_Paolina 0:ff94cc47fef7 46
Fairy_Paolina 0:ff94cc47fef7 47 wall_follow();
Fairy_Paolina 0:ff94cc47fef7 48
Fairy_Paolina 0:ff94cc47fef7 49 motors.stopBothMotors();
Fairy_Paolina 0:ff94cc47fef7 50
Fairy_Paolina 0:ff94cc47fef7 51
Fairy_Paolina 0:ff94cc47fef7 52 }
Fairy_Paolina 0:ff94cc47fef7 53
Fairy_Paolina 0:ff94cc47fef7 54 void errFunction(void){
Fairy_Paolina 0:ff94cc47fef7 55 //Nothing
Fairy_Paolina 0:ff94cc47fef7 56 }
Fairy_Paolina 0:ff94cc47fef7 57
Fairy_Paolina 0:ff94cc47fef7 58 void us_distance(void)
Fairy_Paolina 0:ff94cc47fef7 59 {
Fairy_Paolina 0:ff94cc47fef7 60 pc.printf("Ultra Sonic\n\r");
Fairy_Paolina 0:ff94cc47fef7 61 rangeFinder.startMeas();
Fairy_Paolina 0:ff94cc47fef7 62 wait_us(20);
Fairy_Paolina 0:ff94cc47fef7 63 if ( (rangeFinder.getMeas(range) == RANGE_MEAS_VALID))
Fairy_Paolina 0:ff94cc47fef7 64 {
Fairy_Paolina 0:ff94cc47fef7 65 pc.printf("Range = %f\n\r", range);
Fairy_Paolina 0:ff94cc47fef7 66 }
Fairy_Paolina 0:ff94cc47fef7 67 }
Fairy_Paolina 0:ff94cc47fef7 68
Fairy_Paolina 0:ff94cc47fef7 69 void wall_follow(void)
Fairy_Paolina 0:ff94cc47fef7 70 {
Fairy_Paolina 0:ff94cc47fef7 71 while(1){
Fairy_Paolina 0:ff94cc47fef7 72
Fairy_Paolina 0:ff94cc47fef7 73 pid1.setInputLimits(1.0, 20.0);
Fairy_Paolina 0:ff94cc47fef7 74 pid1.setOutputLimits( -0.3*127, 0.3*127);
Fairy_Paolina 0:ff94cc47fef7 75 pid1.setSetPoint(10.0);
Fairy_Paolina 0:ff94cc47fef7 76
Fairy_Paolina 0:ff94cc47fef7 77 rangeFinder.startMeas();
Fairy_Paolina 0:ff94cc47fef7 78 wait_ms(100);
Fairy_Paolina 0:ff94cc47fef7 79 if ( (rangeFinder.getMeas(range) == RANGE_MEAS_VALID) && (range < 100.0) && (range > 3.0))
Fairy_Paolina 0:ff94cc47fef7 80 {
Fairy_Paolina 0:ff94cc47fef7 81 //pc.printf("Range = %f\n", range);
Fairy_Paolina 0:ff94cc47fef7 82 }
Fairy_Paolina 0:ff94cc47fef7 83 pid1.setProcessValue(range);
Fairy_Paolina 0:ff94cc47fef7 84 pid_return = pid1.compute();
Fairy_Paolina 0:ff94cc47fef7 85 pc.printf("Range: %f\n PID: %f", range, pid_return);
Fairy_Paolina 0:ff94cc47fef7 86
Fairy_Paolina 0:ff94cc47fef7 87 if(pid_return > 0){
Fairy_Paolina 0:ff94cc47fef7 88 motors.setMotor0Speed(MAX_SPEED - pid_return);//left
Fairy_Paolina 0:ff94cc47fef7 89 motors.setMotor1Speed(MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 90 }else if(pid_return < 0){
Fairy_Paolina 0:ff94cc47fef7 91 motors.setMotor0Speed(MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 92 motors.setMotor1Speed(MAX_SPEED + pid_return);
Fairy_Paolina 0:ff94cc47fef7 93 }else{
Fairy_Paolina 0:ff94cc47fef7 94 motors.setMotor0Speed(MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 95 motors.setMotor1Speed(MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 96 }
Fairy_Paolina 0:ff94cc47fef7 97 }
Fairy_Paolina 0:ff94cc47fef7 98 }
Fairy_Paolina 0:ff94cc47fef7 99
Fairy_Paolina 0:ff94cc47fef7 100 /* MODIFIED WALL_FOLLOW FOR NAVIGATION */
Fairy_Paolina 0:ff94cc47fef7 101
Fairy_Paolina 0:ff94cc47fef7 102 void wall_follow2(int &currentLocation)
Fairy_Paolina 0:ff94cc47fef7 103 {
Fairy_Paolina 0:ff94cc47fef7 104 int SeeWaveGap = false;
Fairy_Paolina 0:ff94cc47fef7 105 while(1){
Fairy_Paolina 0:ff94cc47fef7 106
Fairy_Paolina 0:ff94cc47fef7 107 pid1.setInputLimits(4.0, 20.0);
Fairy_Paolina 0:ff94cc47fef7 108 pid1.setOutputLimits( -0.6, 0.6);
Fairy_Paolina 0:ff94cc47fef7 109 pid1.setSetPoint(20.0);
Fairy_Paolina 0:ff94cc47fef7 110
Fairy_Paolina 0:ff94cc47fef7 111 rangeFinder.startMeas();
Fairy_Paolina 0:ff94cc47fef7 112 wait_ms(100);
Fairy_Paolina 0:ff94cc47fef7 113 if ( (rangeFinder.getMeas(range) == RANGE_MEAS_VALID) && (range < 100.0) && (range > 3.0))
Fairy_Paolina 0:ff94cc47fef7 114 {
Fairy_Paolina 0:ff94cc47fef7 115 //bt.printf("Range = %f\n", range);
Fairy_Paolina 0:ff94cc47fef7 116 }
Fairy_Paolina 0:ff94cc47fef7 117
Fairy_Paolina 0:ff94cc47fef7 118 /*************CHECK FOR WAVE OPENING*****************/
Fairy_Paolina 0:ff94cc47fef7 119 /* If after 100 ms the ultrasonic still sees 20+ cm */
Fairy_Paolina 0:ff94cc47fef7 120 /* then robot is at wave opening */
Fairy_Paolina 0:ff94cc47fef7 121
Fairy_Paolina 0:ff94cc47fef7 122
Fairy_Paolina 0:ff94cc47fef7 123 if(range > 20 && !SeeWaveGap){
Fairy_Paolina 0:ff94cc47fef7 124 currentLocation++;
Fairy_Paolina 0:ff94cc47fef7 125 SeeWaveGap = true;
Fairy_Paolina 0:ff94cc47fef7 126 } else {
Fairy_Paolina 0:ff94cc47fef7 127 // AT WAVE OPENING!!!!
Fairy_Paolina 0:ff94cc47fef7 128 break;
Fairy_Paolina 0:ff94cc47fef7 129 }
Fairy_Paolina 0:ff94cc47fef7 130
Fairy_Paolina 0:ff94cc47fef7 131
Fairy_Paolina 0:ff94cc47fef7 132 pid1.setProcessValue(range);
Fairy_Paolina 0:ff94cc47fef7 133 pid_return = pid1.compute();
Fairy_Paolina 0:ff94cc47fef7 134 bt.printf("Range: %f\n PID: %f", range, pid_return);
Fairy_Paolina 0:ff94cc47fef7 135
Fairy_Paolina 0:ff94cc47fef7 136 if(pid_return > 0){
Fairy_Paolina 0:ff94cc47fef7 137 motors.setMotor0Speed(MAX_SPEED - pid_return);
Fairy_Paolina 0:ff94cc47fef7 138 motors.setMotor1Speed(MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 139 }else if(pid_return < 0){
Fairy_Paolina 0:ff94cc47fef7 140 motors.setMotor0Speed(MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 141 motors.setMotor1Speed(MAX_SPEED + pid_return);
Fairy_Paolina 0:ff94cc47fef7 142 }else{
Fairy_Paolina 0:ff94cc47fef7 143 motors.setMotor0Speed(MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 144 motors.setMotor1Speed(MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 145 }
Fairy_Paolina 0:ff94cc47fef7 146 }
Fairy_Paolina 0:ff94cc47fef7 147 }
Fairy_Paolina 0:ff94cc47fef7 148
Fairy_Paolina 0:ff94cc47fef7 149
Fairy_Paolina 0:ff94cc47fef7 150 /* MODIFIED WALL_FOLLOW FOR NAVIGATION WITH WAVE OPENINGS PASSED IN */
Fairy_Paolina 0:ff94cc47fef7 151 /* MEANT FOR RETURNING FROM OIL RIGS */
Fairy_Paolina 0:ff94cc47fef7 152
Fairy_Paolina 0:ff94cc47fef7 153 void wall_follow3(int &currentLocation, int &WaveOpening)
Fairy_Paolina 0:ff94cc47fef7 154 {
Fairy_Paolina 0:ff94cc47fef7 155 while(1){
Fairy_Paolina 0:ff94cc47fef7 156
Fairy_Paolina 0:ff94cc47fef7 157
Fairy_Paolina 0:ff94cc47fef7 158 pid1.setInputLimits(4.0, 20.0);
Fairy_Paolina 0:ff94cc47fef7 159 pid1.setOutputLimits( -0.6, 0.6);
Fairy_Paolina 0:ff94cc47fef7 160 pid1.setSetPoint(20.0);
Fairy_Paolina 0:ff94cc47fef7 161
Fairy_Paolina 0:ff94cc47fef7 162 rangeFinder.startMeas();
Fairy_Paolina 0:ff94cc47fef7 163 wait_ms(100);
Fairy_Paolina 0:ff94cc47fef7 164 if ( (rangeFinder.getMeas(range) == RANGE_MEAS_VALID) && (range < 100.0) && (range > 3.0))
Fairy_Paolina 0:ff94cc47fef7 165 {
Fairy_Paolina 0:ff94cc47fef7 166 //bt.printf("Range = %f\n", range);
Fairy_Paolina 0:ff94cc47fef7 167 }
Fairy_Paolina 0:ff94cc47fef7 168
Fairy_Paolina 0:ff94cc47fef7 169 /*************CHECK FOR WAVE OPENING*****************/
Fairy_Paolina 0:ff94cc47fef7 170 /* If after 100 ms the ultrasonic still sees 20+ cm */
Fairy_Paolina 0:ff94cc47fef7 171 /* then robot is at wave opening */
Fairy_Paolina 0:ff94cc47fef7 172
Fairy_Paolina 0:ff94cc47fef7 173
Fairy_Paolina 0:ff94cc47fef7 174 if(range > 20 ){
Fairy_Paolina 0:ff94cc47fef7 175 currentLocation--;
Fairy_Paolina 0:ff94cc47fef7 176 }
Fairy_Paolina 0:ff94cc47fef7 177
Fairy_Paolina 0:ff94cc47fef7 178 if( currentLocation == WaveOpening){
Fairy_Paolina 0:ff94cc47fef7 179 // AT WAVE OPENING!!!!
Fairy_Paolina 0:ff94cc47fef7 180
Fairy_Paolina 0:ff94cc47fef7 181 break;
Fairy_Paolina 0:ff94cc47fef7 182 }
Fairy_Paolina 0:ff94cc47fef7 183
Fairy_Paolina 0:ff94cc47fef7 184
Fairy_Paolina 0:ff94cc47fef7 185 pid1.setProcessValue(range);
Fairy_Paolina 0:ff94cc47fef7 186 pid_return = pid1.compute();
Fairy_Paolina 0:ff94cc47fef7 187 bt.printf("Range: %f\n PID: %f", range, pid_return);
Fairy_Paolina 0:ff94cc47fef7 188
Fairy_Paolina 0:ff94cc47fef7 189 if(pid_return > 0){
Fairy_Paolina 0:ff94cc47fef7 190 motors.setMotor0Speed(MAX_SPEED - pid_return);
Fairy_Paolina 0:ff94cc47fef7 191 motors.setMotor1Speed(MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 192 }else if(pid_return < 0){
Fairy_Paolina 0:ff94cc47fef7 193 motors.setMotor0Speed(MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 194 motors.setMotor1Speed(MAX_SPEED + pid_return);
Fairy_Paolina 0:ff94cc47fef7 195 }else{
Fairy_Paolina 0:ff94cc47fef7 196 motors.setMotor0Speed(MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 197 motors.setMotor1Speed(MAX_SPEED);
Fairy_Paolina 0:ff94cc47fef7 198 }
Fairy_Paolina 0:ff94cc47fef7 199 }
Fairy_Paolina 0:ff94cc47fef7 200 }