Demo program for the Marvino robot from Fortito (on board processor)
main.cpp@0:3c55e8a81dfb, 2016-02-02 (annotated)
- Committer:
- diegorivera42
- Date:
- Tue Feb 02 12:04:37 2016 +0000
- Revision:
- 0:3c55e8a81dfb
Demo version 1
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
diegorivera42 | 0:3c55e8a81dfb | 1 | #include "mbed.h" |
diegorivera42 | 0:3c55e8a81dfb | 2 | |
diegorivera42 | 0:3c55e8a81dfb | 3 | Timer t1; |
diegorivera42 | 0:3c55e8a81dfb | 4 | Timer t2; |
diegorivera42 | 0:3c55e8a81dfb | 5 | |
diegorivera42 | 0:3c55e8a81dfb | 6 | DigitalOut myled(LED1); |
diegorivera42 | 0:3c55e8a81dfb | 7 | I2C i2c(p9, p10); |
diegorivera42 | 0:3c55e8a81dfb | 8 | AnalogIn light(p18); |
diegorivera42 | 0:3c55e8a81dfb | 9 | AnalogIn linel(p19); |
diegorivera42 | 0:3c55e8a81dfb | 10 | AnalogIn liner(p20); |
diegorivera42 | 0:3c55e8a81dfb | 11 | PwmOut eyes(p21); |
diegorivera42 | 0:3c55e8a81dfb | 12 | PwmOut motlspd(p22); |
diegorivera42 | 0:3c55e8a81dfb | 13 | PwmOut motrspd(p23); |
diegorivera42 | 0:3c55e8a81dfb | 14 | DigitalOut motldir(p24); |
diegorivera42 | 0:3c55e8a81dfb | 15 | DigitalOut motrdir(p25); |
diegorivera42 | 0:3c55e8a81dfb | 16 | DigitalOut speaker(p26); |
diegorivera42 | 0:3c55e8a81dfb | 17 | DigitalIn armr(p27); |
diegorivera42 | 0:3c55e8a81dfb | 18 | DigitalIn armrf(p28); |
diegorivera42 | 0:3c55e8a81dfb | 19 | DigitalIn arml(p29); |
diegorivera42 | 0:3c55e8a81dfb | 20 | DigitalIn armlf(p30); |
diegorivera42 | 0:3c55e8a81dfb | 21 | |
diegorivera42 | 0:3c55e8a81dfb | 22 | int HatLightAddr = 0x46; // PCA8575 16 bit IO |
diegorivera42 | 0:3c55e8a81dfb | 23 | char HatLightData[2]; // 16 bit data |
diegorivera42 | 0:3c55e8a81dfb | 24 | float PWMFreq = 5000; // PWM Frequency in Hz |
diegorivera42 | 0:3c55e8a81dfb | 25 | float PWMPeriod = 1 / PWMFreq; // On the mbed this is global for all PWM output pins |
diegorivera42 | 0:3c55e8a81dfb | 26 | float MotLeftAdj = 1; // Left motor speed adjust |
diegorivera42 | 0:3c55e8a81dfb | 27 | float MotRightAdj = 1; // Right motor speed adjust |
diegorivera42 | 0:3c55e8a81dfb | 28 | float vcc = 3.3; // mbed supply voltage |
diegorivera42 | 0:3c55e8a81dfb | 29 | float ForwardFull = 1; // motor forward full speed |
diegorivera42 | 0:3c55e8a81dfb | 30 | float ForwardNorm = ForwardFull * 0.5; // forward normal speed |
diegorivera42 | 0:3c55e8a81dfb | 31 | float ForwardSlow = ForwardNorm * 0.5; // forward half normal speed |
diegorivera42 | 0:3c55e8a81dfb | 32 | float ReverseNorm = 0-ForwardNorm; // reverse normal speed |
diegorivera42 | 0:3c55e8a81dfb | 33 | float ReverseFull = 0-ForwardFull; // motor reverse full speed |
diegorivera42 | 0:3c55e8a81dfb | 34 | float ReverseSlow = 0-ForwardSlow; // reverse half normal speed |
diegorivera42 | 0:3c55e8a81dfb | 35 | float Stop = 0.0; // stop |
diegorivera42 | 0:3c55e8a81dfb | 36 | float SensorTimeOut1 = 10; // Nothing happening to Arms or Line Following in seconds (change to line following) |
diegorivera42 | 0:3c55e8a81dfb | 37 | float SensorTimeOut2 = 10; // Nothing happening to Light Following in seconds (stuck? so reverse turn) |
diegorivera42 | 0:3c55e8a81dfb | 38 | int LightChange = 0; // used to determine change in light following |
diegorivera42 | 0:3c55e8a81dfb | 39 | int LineFollow = 0; // used to determine continous run of line following |
diegorivera42 | 0:3c55e8a81dfb | 40 | int LineBright = 1; // voltage that determines line brightness |
diegorivera42 | 0:3c55e8a81dfb | 41 | |
diegorivera42 | 0:3c55e8a81dfb | 42 | // MotorLeft expects -1 to 1 float for speed and direction |
diegorivera42 | 0:3c55e8a81dfb | 43 | void MotorLeft(float sp) { |
diegorivera42 | 0:3c55e8a81dfb | 44 | if (sp > 0) { |
diegorivera42 | 0:3c55e8a81dfb | 45 | motldir = 1; |
diegorivera42 | 0:3c55e8a81dfb | 46 | } |
diegorivera42 | 0:3c55e8a81dfb | 47 | else { |
diegorivera42 | 0:3c55e8a81dfb | 48 | motldir = 0; |
diegorivera42 | 0:3c55e8a81dfb | 49 | sp = 0 - sp; |
diegorivera42 | 0:3c55e8a81dfb | 50 | } |
diegorivera42 | 0:3c55e8a81dfb | 51 | motlspd.pulsewidth(sp * PWMPeriod * MotLeftAdj); // left motor a bit fast so slow it down |
diegorivera42 | 0:3c55e8a81dfb | 52 | } |
diegorivera42 | 0:3c55e8a81dfb | 53 | |
diegorivera42 | 0:3c55e8a81dfb | 54 | // MotorRight expects -1 to 1 float for speed and direction |
diegorivera42 | 0:3c55e8a81dfb | 55 | void MotorRight(float sp) { |
diegorivera42 | 0:3c55e8a81dfb | 56 | if (sp > 0) { |
diegorivera42 | 0:3c55e8a81dfb | 57 | motrdir = 1; |
diegorivera42 | 0:3c55e8a81dfb | 58 | } |
diegorivera42 | 0:3c55e8a81dfb | 59 | else { |
diegorivera42 | 0:3c55e8a81dfb | 60 | motrdir = 0; |
diegorivera42 | 0:3c55e8a81dfb | 61 | sp = 0 - sp; |
diegorivera42 | 0:3c55e8a81dfb | 62 | } |
diegorivera42 | 0:3c55e8a81dfb | 63 | motrspd.pulsewidth(sp * PWMPeriod * MotRightAdj); |
diegorivera42 | 0:3c55e8a81dfb | 64 | } |
diegorivera42 | 0:3c55e8a81dfb | 65 | |
diegorivera42 | 0:3c55e8a81dfb | 66 | // EyesBright expects 0 to 1 float for brrightness |
diegorivera42 | 0:3c55e8a81dfb | 67 | void EyesBright(float sp) { |
diegorivera42 | 0:3c55e8a81dfb | 68 | eyes.pulsewidth(sp * PWMPeriod); |
diegorivera42 | 0:3c55e8a81dfb | 69 | } |
diegorivera42 | 0:3c55e8a81dfb | 70 | |
diegorivera42 | 0:3c55e8a81dfb | 71 | void HatLights(int d) { |
diegorivera42 | 0:3c55e8a81dfb | 72 | d=d ^ 0xffff; |
diegorivera42 | 0:3c55e8a81dfb | 73 | HatLightData[0]=(d); |
diegorivera42 | 0:3c55e8a81dfb | 74 | HatLightData[1]=(d >>8); |
diegorivera42 | 0:3c55e8a81dfb | 75 | i2c.write( HatLightAddr, HatLightData, 2 ); |
diegorivera42 | 0:3c55e8a81dfb | 76 | } |
diegorivera42 | 0:3c55e8a81dfb | 77 | |
diegorivera42 | 0:3c55e8a81dfb | 78 | unsigned int m_z=12434,m_w=33254; |
diegorivera42 | 0:3c55e8a81dfb | 79 | |
diegorivera42 | 0:3c55e8a81dfb | 80 | unsigned int rnd() { |
diegorivera42 | 0:3c55e8a81dfb | 81 | m_z = 36969 * (m_z & 65535) + (m_z >>16); |
diegorivera42 | 0:3c55e8a81dfb | 82 | m_w = 18000 * (m_w & 65535) + (m_w >>16); |
diegorivera42 | 0:3c55e8a81dfb | 83 | return ((m_z <<16) + m_w); |
diegorivera42 | 0:3c55e8a81dfb | 84 | } |
diegorivera42 | 0:3c55e8a81dfb | 85 | |
diegorivera42 | 0:3c55e8a81dfb | 86 | // Idea sets hat lights to a random pattern n times at period in seconds |
diegorivera42 | 0:3c55e8a81dfb | 87 | void Idea(int n, float p) { |
diegorivera42 | 0:3c55e8a81dfb | 88 | for (int i=0; i<n; i++) { |
diegorivera42 | 0:3c55e8a81dfb | 89 | HatLights(rnd()%0x8000); |
diegorivera42 | 0:3c55e8a81dfb | 90 | wait(p); |
diegorivera42 | 0:3c55e8a81dfb | 91 | } |
diegorivera42 | 0:3c55e8a81dfb | 92 | HatLights(0); |
diegorivera42 | 0:3c55e8a81dfb | 93 | } |
diegorivera42 | 0:3c55e8a81dfb | 94 | |
diegorivera42 | 0:3c55e8a81dfb | 95 | // Thinking rotates hat lights n times at period p in seconds |
diegorivera42 | 0:3c55e8a81dfb | 96 | void Thinking(int n, float p) { |
diegorivera42 | 0:3c55e8a81dfb | 97 | int r=1; |
diegorivera42 | 0:3c55e8a81dfb | 98 | for (int i=0; i<n; i++) { |
diegorivera42 | 0:3c55e8a81dfb | 99 | HatLights(r); |
diegorivera42 | 0:3c55e8a81dfb | 100 | wait(p); |
diegorivera42 | 0:3c55e8a81dfb | 101 | if (r == 0x8000) { |
diegorivera42 | 0:3c55e8a81dfb | 102 | r=1; |
diegorivera42 | 0:3c55e8a81dfb | 103 | } |
diegorivera42 | 0:3c55e8a81dfb | 104 | else { |
diegorivera42 | 0:3c55e8a81dfb | 105 | r=(r <<1); |
diegorivera42 | 0:3c55e8a81dfb | 106 | } |
diegorivera42 | 0:3c55e8a81dfb | 107 | } |
diegorivera42 | 0:3c55e8a81dfb | 108 | HatLights(0); |
diegorivera42 | 0:3c55e8a81dfb | 109 | } |
diegorivera42 | 0:3c55e8a81dfb | 110 | |
diegorivera42 | 0:3c55e8a81dfb | 111 | // Beep sounder at frequency f (cycles/sec) for period p in seconds |
diegorivera42 | 0:3c55e8a81dfb | 112 | void Beep(float f, float p) { |
diegorivera42 | 0:3c55e8a81dfb | 113 | float t = 0.5/f; |
diegorivera42 | 0:3c55e8a81dfb | 114 | for (int i=0; i<(f*p); i++) { |
diegorivera42 | 0:3c55e8a81dfb | 115 | speaker = 1; // speaker high |
diegorivera42 | 0:3c55e8a81dfb | 116 | wait(t); |
diegorivera42 | 0:3c55e8a81dfb | 117 | speaker = 0; // speaker low |
diegorivera42 | 0:3c55e8a81dfb | 118 | wait(t); |
diegorivera42 | 0:3c55e8a81dfb | 119 | } |
diegorivera42 | 0:3c55e8a81dfb | 120 | } |
diegorivera42 | 0:3c55e8a81dfb | 121 | |
diegorivera42 | 0:3c55e8a81dfb | 122 | // FlashEyes flashes eyes n times at period p in seconds |
diegorivera42 | 0:3c55e8a81dfb | 123 | void FlashEyes(int n, float p) { |
diegorivera42 | 0:3c55e8a81dfb | 124 | for (int i=0; i<n; i++) { |
diegorivera42 | 0:3c55e8a81dfb | 125 | EyesBright(1); // eyes full on |
diegorivera42 | 0:3c55e8a81dfb | 126 | wait(p/2); |
diegorivera42 | 0:3c55e8a81dfb | 127 | EyesBright(0); // eyes off |
diegorivera42 | 0:3c55e8a81dfb | 128 | wait(p/2); |
diegorivera42 | 0:3c55e8a81dfb | 129 | } |
diegorivera42 | 0:3c55e8a81dfb | 130 | EyesBright(0.5); // back to half bright |
diegorivera42 | 0:3c55e8a81dfb | 131 | } |
diegorivera42 | 0:3c55e8a81dfb | 132 | |
diegorivera42 | 0:3c55e8a81dfb | 133 | void AvoidLeft() { |
diegorivera42 | 0:3c55e8a81dfb | 134 | MotorLeft(Stop); |
diegorivera42 | 0:3c55e8a81dfb | 135 | MotorRight(Stop); // stop |
diegorivera42 | 0:3c55e8a81dfb | 136 | Thinking(16, 0.02); // rotate hat lights |
diegorivera42 | 0:3c55e8a81dfb | 137 | MotorLeft(ReverseSlow); |
diegorivera42 | 0:3c55e8a81dfb | 138 | MotorRight(ReverseNorm); // reverse turn back |
diegorivera42 | 0:3c55e8a81dfb | 139 | FlashEyes(3, 0.1); |
diegorivera42 | 0:3c55e8a81dfb | 140 | MotorLeft(ForwardNorm); |
diegorivera42 | 0:3c55e8a81dfb | 141 | MotorRight(ForwardNorm); // forward |
diegorivera42 | 0:3c55e8a81dfb | 142 | } |
diegorivera42 | 0:3c55e8a81dfb | 143 | |
diegorivera42 | 0:3c55e8a81dfb | 144 | void AvoidRight() { |
diegorivera42 | 0:3c55e8a81dfb | 145 | MotorLeft(Stop); |
diegorivera42 | 0:3c55e8a81dfb | 146 | MotorRight(Stop); // stop |
diegorivera42 | 0:3c55e8a81dfb | 147 | Thinking(16, 0.02); // rotate hat lights |
diegorivera42 | 0:3c55e8a81dfb | 148 | MotorLeft(ReverseNorm); |
diegorivera42 | 0:3c55e8a81dfb | 149 | MotorRight(ReverseSlow); // reverse turn back |
diegorivera42 | 0:3c55e8a81dfb | 150 | FlashEyes(3, 0.1); |
diegorivera42 | 0:3c55e8a81dfb | 151 | MotorLeft(ForwardNorm); |
diegorivera42 | 0:3c55e8a81dfb | 152 | MotorRight(ForwardNorm); // forward |
diegorivera42 | 0:3c55e8a81dfb | 153 | } |
diegorivera42 | 0:3c55e8a81dfb | 154 | |
diegorivera42 | 0:3c55e8a81dfb | 155 | void AvoidLeftFront() { |
diegorivera42 | 0:3c55e8a81dfb | 156 | MotorLeft(Stop); |
diegorivera42 | 0:3c55e8a81dfb | 157 | MotorRight(Stop); // stop |
diegorivera42 | 0:3c55e8a81dfb | 158 | Thinking(16, 0.02); // rotate hat lights |
diegorivera42 | 0:3c55e8a81dfb | 159 | MotorLeft(ReverseNorm); |
diegorivera42 | 0:3c55e8a81dfb | 160 | MotorRight(ReverseNorm); // reverse |
diegorivera42 | 0:3c55e8a81dfb | 161 | FlashEyes(6, 0.1); // flash eyes |
diegorivera42 | 0:3c55e8a81dfb | 162 | MotorRight(Stop); |
diegorivera42 | 0:3c55e8a81dfb | 163 | FlashEyes(6, 0.1); // rotate |
diegorivera42 | 0:3c55e8a81dfb | 164 | MotorLeft(ForwardNorm); |
diegorivera42 | 0:3c55e8a81dfb | 165 | MotorRight(ForwardNorm); // forward |
diegorivera42 | 0:3c55e8a81dfb | 166 | } |
diegorivera42 | 0:3c55e8a81dfb | 167 | |
diegorivera42 | 0:3c55e8a81dfb | 168 | void AvoidRightFront() { |
diegorivera42 | 0:3c55e8a81dfb | 169 | MotorLeft(Stop); |
diegorivera42 | 0:3c55e8a81dfb | 170 | MotorRight(Stop); // stop |
diegorivera42 | 0:3c55e8a81dfb | 171 | Thinking(16, 0.02); // rotate hat lights |
diegorivera42 | 0:3c55e8a81dfb | 172 | MotorLeft(ReverseNorm); |
diegorivera42 | 0:3c55e8a81dfb | 173 | MotorRight(ReverseNorm); // reverse |
diegorivera42 | 0:3c55e8a81dfb | 174 | FlashEyes(6, 0.1); // flash eyes |
diegorivera42 | 0:3c55e8a81dfb | 175 | MotorLeft(Stop); |
diegorivera42 | 0:3c55e8a81dfb | 176 | FlashEyes(6, 0.1); // rotate |
diegorivera42 | 0:3c55e8a81dfb | 177 | MotorLeft(ForwardNorm); |
diegorivera42 | 0:3c55e8a81dfb | 178 | MotorRight(ForwardNorm); // forward |
diegorivera42 | 0:3c55e8a81dfb | 179 | } |
diegorivera42 | 0:3c55e8a81dfb | 180 | |
diegorivera42 | 0:3c55e8a81dfb | 181 | void AvoidHeadOn() { |
diegorivera42 | 0:3c55e8a81dfb | 182 | MotorLeft(Stop); |
diegorivera42 | 0:3c55e8a81dfb | 183 | MotorRight(Stop); // stop |
diegorivera42 | 0:3c55e8a81dfb | 184 | Thinking(50, 0.02); // rotate hat lights |
diegorivera42 | 0:3c55e8a81dfb | 185 | Idea(20, 0.02); // flash random hat lights |
diegorivera42 | 0:3c55e8a81dfb | 186 | MotorLeft(ReverseNorm); |
diegorivera42 | 0:3c55e8a81dfb | 187 | MotorRight(ReverseNorm); // reverse |
diegorivera42 | 0:3c55e8a81dfb | 188 | FlashEyes(8, 0.1); // flash eyes |
diegorivera42 | 0:3c55e8a81dfb | 189 | MotorLeft(ForwardNorm); |
diegorivera42 | 0:3c55e8a81dfb | 190 | FlashEyes(8, 0.1); // rotate clockwise |
diegorivera42 | 0:3c55e8a81dfb | 191 | MotorRight(ForwardNorm); // carry on straight |
diegorivera42 | 0:3c55e8a81dfb | 192 | } |
diegorivera42 | 0:3c55e8a81dfb | 193 | |
diegorivera42 | 0:3c55e8a81dfb | 194 | void NudgeLeft() { |
diegorivera42 | 0:3c55e8a81dfb | 195 | MotorLeft(Stop); // stop left motor |
diegorivera42 | 0:3c55e8a81dfb | 196 | wait(0.01); // wait a bit |
diegorivera42 | 0:3c55e8a81dfb | 197 | MotorLeft(ForwardNorm); // carry on |
diegorivera42 | 0:3c55e8a81dfb | 198 | } |
diegorivera42 | 0:3c55e8a81dfb | 199 | |
diegorivera42 | 0:3c55e8a81dfb | 200 | void NudgeRight() { |
diegorivera42 | 0:3c55e8a81dfb | 201 | MotorRight(Stop); // stop right motor |
diegorivera42 | 0:3c55e8a81dfb | 202 | wait(0.01); // wait a bit |
diegorivera42 | 0:3c55e8a81dfb | 203 | MotorRight(ForwardNorm); // carry on |
diegorivera42 | 0:3c55e8a81dfb | 204 | } |
diegorivera42 | 0:3c55e8a81dfb | 205 | |
diegorivera42 | 0:3c55e8a81dfb | 206 | int main() { |
diegorivera42 | 0:3c55e8a81dfb | 207 | motlspd.period(PWMPeriod); |
diegorivera42 | 0:3c55e8a81dfb | 208 | motrspd.period(PWMPeriod); |
diegorivera42 | 0:3c55e8a81dfb | 209 | eyes.period(PWMPeriod); |
diegorivera42 | 0:3c55e8a81dfb | 210 | EyesBright(0.5); // eyes half brightness |
diegorivera42 | 0:3c55e8a81dfb | 211 | MotorLeft(ForwardNorm); // motor left forward |
diegorivera42 | 0:3c55e8a81dfb | 212 | MotorRight(ForwardNorm); // motor right forward |
diegorivera42 | 0:3c55e8a81dfb | 213 | t1.start(); |
diegorivera42 | 0:3c55e8a81dfb | 214 | while(1) { |
diegorivera42 | 0:3c55e8a81dfb | 215 | if (arml == 0 && armr != 0) { |
diegorivera42 | 0:3c55e8a81dfb | 216 | t1.reset(); |
diegorivera42 | 0:3c55e8a81dfb | 217 | AvoidLeft(); |
diegorivera42 | 0:3c55e8a81dfb | 218 | } |
diegorivera42 | 0:3c55e8a81dfb | 219 | if (arml != 0 && armr == 0) { |
diegorivera42 | 0:3c55e8a81dfb | 220 | t1.reset(); |
diegorivera42 | 0:3c55e8a81dfb | 221 | AvoidRight(); |
diegorivera42 | 0:3c55e8a81dfb | 222 | } |
diegorivera42 | 0:3c55e8a81dfb | 223 | if (arml == 0 && armr == 0) { |
diegorivera42 | 0:3c55e8a81dfb | 224 | t1.reset(); |
diegorivera42 | 0:3c55e8a81dfb | 225 | AvoidHeadOn(); |
diegorivera42 | 0:3c55e8a81dfb | 226 | } |
diegorivera42 | 0:3c55e8a81dfb | 227 | if (armlf == 0 && armrf == 0) { |
diegorivera42 | 0:3c55e8a81dfb | 228 | t1.reset(); |
diegorivera42 | 0:3c55e8a81dfb | 229 | AvoidHeadOn(); |
diegorivera42 | 0:3c55e8a81dfb | 230 | } |
diegorivera42 | 0:3c55e8a81dfb | 231 | if (armlf == 0 && armrf != 0) { |
diegorivera42 | 0:3c55e8a81dfb | 232 | t1.reset(); |
diegorivera42 | 0:3c55e8a81dfb | 233 | AvoidLeftFront(); |
diegorivera42 | 0:3c55e8a81dfb | 234 | } |
diegorivera42 | 0:3c55e8a81dfb | 235 | if (armlf != 0 && armrf == 0) { |
diegorivera42 | 0:3c55e8a81dfb | 236 | t1.reset(); |
diegorivera42 | 0:3c55e8a81dfb | 237 | AvoidRightFront(); |
diegorivera42 | 0:3c55e8a81dfb | 238 | } |
diegorivera42 | 0:3c55e8a81dfb | 239 | if ((linel > (LineBright/vcc)) && (liner < (LineBright/vcc))) { |
diegorivera42 | 0:3c55e8a81dfb | 240 | NudgeLeft(); |
diegorivera42 | 0:3c55e8a81dfb | 241 | } |
diegorivera42 | 0:3c55e8a81dfb | 242 | if ((linel < (LineBright/vcc)) && (liner > (LineBright/vcc))) { |
diegorivera42 | 0:3c55e8a81dfb | 243 | NudgeRight(); |
diegorivera42 | 0:3c55e8a81dfb | 244 | } |
diegorivera42 | 0:3c55e8a81dfb | 245 | if (((linel < (LineBright/vcc)) && (liner < (LineBright/vcc))) || ((linel > (LineBright/vcc)) && (liner > (LineBright/vcc)))) { |
diegorivera42 | 0:3c55e8a81dfb | 246 | LineFollow = 0; |
diegorivera42 | 0:3c55e8a81dfb | 247 | } |
diegorivera42 | 0:3c55e8a81dfb | 248 | else { |
diegorivera42 | 0:3c55e8a81dfb | 249 | LineFollow++; |
diegorivera42 | 0:3c55e8a81dfb | 250 | if (LineFollow > 10) { |
diegorivera42 | 0:3c55e8a81dfb | 251 | t1.reset(); // only reset activity timer if consistant line following |
diegorivera42 | 0:3c55e8a81dfb | 252 | } |
diegorivera42 | 0:3c55e8a81dfb | 253 | } |
diegorivera42 | 0:3c55e8a81dfb | 254 | if (t1.read() > SensorTimeOut1) { // if nothing else is happening then follow light |
diegorivera42 | 0:3c55e8a81dfb | 255 | t2.start(); |
diegorivera42 | 0:3c55e8a81dfb | 256 | if (light > 0.7) { // more light to the left |
diegorivera42 | 0:3c55e8a81dfb | 257 | if (LightChange == 0) { |
diegorivera42 | 0:3c55e8a81dfb | 258 | LightChange = 1; |
diegorivera42 | 0:3c55e8a81dfb | 259 | t2.reset(); |
diegorivera42 | 0:3c55e8a81dfb | 260 | Beep(4000,0.1); |
diegorivera42 | 0:3c55e8a81dfb | 261 | } |
diegorivera42 | 0:3c55e8a81dfb | 262 | NudgeLeft(); |
diegorivera42 | 0:3c55e8a81dfb | 263 | } |
diegorivera42 | 0:3c55e8a81dfb | 264 | if (light < 0.3) { // more light to the right |
diegorivera42 | 0:3c55e8a81dfb | 265 | if (LightChange == 1) { |
diegorivera42 | 0:3c55e8a81dfb | 266 | LightChange = 0; |
diegorivera42 | 0:3c55e8a81dfb | 267 | t2.reset(); |
diegorivera42 | 0:3c55e8a81dfb | 268 | Beep(1000,0.1); |
diegorivera42 | 0:3c55e8a81dfb | 269 | } |
diegorivera42 | 0:3c55e8a81dfb | 270 | NudgeRight(); |
diegorivera42 | 0:3c55e8a81dfb | 271 | } |
diegorivera42 | 0:3c55e8a81dfb | 272 | } |
diegorivera42 | 0:3c55e8a81dfb | 273 | else { |
diegorivera42 | 0:3c55e8a81dfb | 274 | t2.reset(); |
diegorivera42 | 0:3c55e8a81dfb | 275 | t2.stop(); |
diegorivera42 | 0:3c55e8a81dfb | 276 | } |
diegorivera42 | 0:3c55e8a81dfb | 277 | if (t2.read() > SensorTimeOut2) { // probably stuck |
diegorivera42 | 0:3c55e8a81dfb | 278 | AvoidHeadOn(); |
diegorivera42 | 0:3c55e8a81dfb | 279 | t1.reset(); |
diegorivera42 | 0:3c55e8a81dfb | 280 | t2.reset(); |
diegorivera42 | 0:3c55e8a81dfb | 281 | t2.stop(); |
diegorivera42 | 0:3c55e8a81dfb | 282 | } |
diegorivera42 | 0:3c55e8a81dfb | 283 | } |
diegorivera42 | 0:3c55e8a81dfb | 284 | } |