Alphabot2-AR line tracking
Dependencies: WS2812 RemoteIR Motor PixelArray Adafruit_GFX Ultrasonica
main.cpp@97:908593afa823, 2019-06-13 (annotated)
- Committer:
- taeyeobyeo
- Date:
- Thu Jun 13 04:52:19 2019 +0000
- Revision:
- 97:908593afa823
- Parent:
- 88:bea4f2daa48c
fixed speed
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
taeyeobyeo | 97:908593afa823 | 1 | #include "mbed.h" |
taeyeobyeo | 97:908593afa823 | 2 | |
taeyeobyeo | 97:908593afa823 | 3 | #include "RemoteIR.h" |
taeyeobyeo | 97:908593afa823 | 4 | #include "ReceiverIR.h" |
taeyeobyeo | 97:908593afa823 | 5 | |
taeyeobyeo | 97:908593afa823 | 6 | #include "Ultrasonic.h" |
taeyeobyeo | 97:908593afa823 | 7 | |
taeyeobyeo | 97:908593afa823 | 8 | #include "Motor.h" |
taeyeobyeo | 97:908593afa823 | 9 | |
taeyeobyeo | 97:908593afa823 | 10 | #include "Adafruit_SSD1306.h" |
taeyeobyeo | 97:908593afa823 | 11 | #include "Adafruit_GFX.h" |
taeyeobyeo | 97:908593afa823 | 12 | #include "glcdfont.h" |
taeyeobyeo | 97:908593afa823 | 13 | #define SSD1306_DISPLAYON 0xAF |
taeyeobyeo | 97:908593afa823 | 14 | |
taeyeobyeo | 97:908593afa823 | 15 | #include "WS2812.h" |
taeyeobyeo | 97:908593afa823 | 16 | #include "PixelArray.h" |
taeyeobyeo | 97:908593afa823 | 17 | |
taeyeobyeo | 97:908593afa823 | 18 | #define WS2812_BUF 150 |
taeyeobyeo | 97:908593afa823 | 19 | #define NUM_COLORS 6 |
taeyeobyeo | 97:908593afa823 | 20 | #define NUM_LEDS_PER_COLOR 10 |
taeyeobyeo | 97:908593afa823 | 21 | |
taeyeobyeo | 97:908593afa823 | 22 | PixelArray px(WS2812_BUF); |
taeyeobyeo | 97:908593afa823 | 23 | WS2812 ws(D7, WS2812_BUF, 0, 5, 5, 0); |
taeyeobyeo | 97:908593afa823 | 24 | |
taeyeobyeo | 97:908593afa823 | 25 | void wsInit(){ |
taeyeobyeo | 97:908593afa823 | 26 | ws.useII(WS2812::PER_PIXEL); |
taeyeobyeo | 97:908593afa823 | 27 | int colorbuf[NUM_COLORS] = {0x2f0000,0x2f2f00,0x002f00,0x002f2f |
taeyeobyeo | 97:908593afa823 | 28 | ,0x00002f,0x2f002f |
taeyeobyeo | 97:908593afa823 | 29 | }; |
taeyeobyeo | 97:908593afa823 | 30 | px.Set(1, 0xFFFFFF); |
taeyeobyeo | 97:908593afa823 | 31 | px.SetI(1, 0xFFFFFF); |
taeyeobyeo | 97:908593afa823 | 32 | } |
taeyeobyeo | 97:908593afa823 | 33 | |
taeyeobyeo | 97:908593afa823 | 34 | Ultrasonic ultra(D3,D2); |
taeyeobyeo | 97:908593afa823 | 35 | Ticker ticker; |
taeyeobyeo | 97:908593afa823 | 36 | |
taeyeobyeo | 97:908593afa823 | 37 | SPI spi(D11, D12, D13); |
taeyeobyeo | 97:908593afa823 | 38 | DigitalOut spi_cs(D10,1); |
taeyeobyeo | 97:908593afa823 | 39 | |
taeyeobyeo | 97:908593afa823 | 40 | RawSerial pc(USBTX, USBRX, 115200); |
taeyeobyeo | 97:908593afa823 | 41 | |
taeyeobyeo | 97:908593afa823 | 42 | Motor right(D5, A3 ,A2); |
taeyeobyeo | 97:908593afa823 | 43 | Motor left(D6, A0 ,A1); |
taeyeobyeo | 97:908593afa823 | 44 | |
taeyeobyeo | 97:908593afa823 | 45 | RemoteIR::Format format = RemoteIR::NEC; |
taeyeobyeo | 97:908593afa823 | 46 | ReceiverIR irrecv(D4); |
taeyeobyeo | 97:908593afa823 | 47 | |
taeyeobyeo | 97:908593afa823 | 48 | Timer timer; |
taeyeobyeo | 97:908593afa823 | 49 | |
taeyeobyeo | 97:908593afa823 | 50 | I2C i2c(D14, D15); |
taeyeobyeo | 97:908593afa823 | 51 | Adafruit_SSD1306_I2c oled(i2c, D9, 0x78, 64, 128); |
taeyeobyeo | 97:908593afa823 | 52 | |
taeyeobyeo | 97:908593afa823 | 53 | uint8_t buf[8]; |
taeyeobyeo | 97:908593afa823 | 54 | |
taeyeobyeo | 97:908593afa823 | 55 | bool white = false, black = false; |
taeyeobyeo | 97:908593afa823 | 56 | |
taeyeobyeo | 97:908593afa823 | 57 | int ir[5]; |
taeyeobyeo | 97:908593afa823 | 58 | int MINIR[5] = {1023,1023,1023,1023,1023}; |
taeyeobyeo | 97:908593afa823 | 59 | int MAXIR[5] = {0,0,0,0,0}; |
taeyeobyeo | 97:908593afa823 | 60 | int lastIR = 0; |
taeyeobyeo | 97:908593afa823 | 61 | |
taeyeobyeo | 97:908593afa823 | 62 | bool run = false; |
mbed_official | 82:abf1b1785bd7 | 63 | |
taeyeobyeo | 97:908593afa823 | 64 | void readLine(){ |
taeyeobyeo | 97:908593afa823 | 65 | int ch = 0; |
taeyeobyeo | 97:908593afa823 | 66 | int value = 0; |
taeyeobyeo | 97:908593afa823 | 67 | // pc.printf("\r\nSPI Value:\r\n"); |
taeyeobyeo | 97:908593afa823 | 68 | do{ |
taeyeobyeo | 97:908593afa823 | 69 | value = 0; |
taeyeobyeo | 97:908593afa823 | 70 | spi_cs = 0; |
taeyeobyeo | 97:908593afa823 | 71 | wait_us(2); |
taeyeobyeo | 97:908593afa823 | 72 | value = spi.write(ch<<12); |
taeyeobyeo | 97:908593afa823 | 73 | spi_cs = 1; |
taeyeobyeo | 97:908593afa823 | 74 | wait_us(18); |
taeyeobyeo | 97:908593afa823 | 75 | switch(ch){ |
taeyeobyeo | 97:908593afa823 | 76 | case 6: |
taeyeobyeo | 97:908593afa823 | 77 | // pc.printf("ch %2d: %.2fmV\r\n",ch, (float)(value >> 6)*3.22580645); break; |
taeyeobyeo | 97:908593afa823 | 78 | case 7: case 8: case 9: case 10: case 11: |
taeyeobyeo | 97:908593afa823 | 79 | case 0: case 12: case 13: case 14: case 15: case 16: break; |
taeyeobyeo | 97:908593afa823 | 80 | case 1: ir[0] = value >> 6;break; |
taeyeobyeo | 97:908593afa823 | 81 | case 2: ir[1] = value >> 6;break; |
taeyeobyeo | 97:908593afa823 | 82 | case 3: ir[2] = value >> 6;break; |
taeyeobyeo | 97:908593afa823 | 83 | case 4: ir[3] = value >> 6;break; |
taeyeobyeo | 97:908593afa823 | 84 | case 5: ir[4] = value >> 6;break; |
taeyeobyeo | 97:908593afa823 | 85 | // default: pc.printf("ch %2d: %d\r\n",ch, value >> 6); |
taeyeobyeo | 97:908593afa823 | 86 | } |
taeyeobyeo | 97:908593afa823 | 87 | // if(ch == 6) pc.printf("ch %2d: %.2fmV\r\n",ch, (float)(value >> 6)*3.22580645); |
taeyeobyeo | 97:908593afa823 | 88 | // else pc.printf("ch %2d: %d\r\n",ch, value >> 6); |
taeyeobyeo | 97:908593afa823 | 89 | ch++; |
taeyeobyeo | 97:908593afa823 | 90 | } while (ch < 6); |
taeyeobyeo | 97:908593afa823 | 91 | } |
Jonathan Austin |
0:2757d7abb7d9 | 92 | |
taeyeobyeo | 97:908593afa823 | 93 | void calibrate(){ |
taeyeobyeo | 97:908593afa823 | 94 | int vMAXIR[5] = {0,0,0,0,0}, vMINIR[5] = {0,0,0,0,0}; |
taeyeobyeo | 97:908593afa823 | 95 | for(int j =0; j< 10; j++){ |
taeyeobyeo | 97:908593afa823 | 96 | readLine(); |
taeyeobyeo | 97:908593afa823 | 97 | for(int i =0;i<5;i++){ |
taeyeobyeo | 97:908593afa823 | 98 | if(j==0 || vMAXIR[i] < ir[i]) vMAXIR[i] = ir[i]; |
taeyeobyeo | 97:908593afa823 | 99 | if(j==0 || vMINIR[i] > ir[i]) vMINIR[i] = ir[i]; |
taeyeobyeo | 97:908593afa823 | 100 | } |
taeyeobyeo | 97:908593afa823 | 101 | } |
taeyeobyeo | 97:908593afa823 | 102 | for (int i =0; i<5; i++){ |
taeyeobyeo | 97:908593afa823 | 103 | if(vMINIR[i] > MAXIR[i]) MAXIR[i] = vMINIR[i]; |
taeyeobyeo | 97:908593afa823 | 104 | if(vMAXIR[i] < MINIR[i]) MINIR[i] = vMAXIR[i]; |
taeyeobyeo | 97:908593afa823 | 105 | } |
taeyeobyeo | 97:908593afa823 | 106 | } |
Jonathan Austin |
0:2757d7abb7d9 | 107 | |
taeyeobyeo | 97:908593afa823 | 108 | void readCalibrated(){ |
taeyeobyeo | 97:908593afa823 | 109 | readLine(); |
taeyeobyeo | 97:908593afa823 | 110 | for(int i =0; i<5;i++){ |
taeyeobyeo | 97:908593afa823 | 111 | int calMINIR, calMAXIR; |
taeyeobyeo | 97:908593afa823 | 112 | int denoMINIRator, x = 0; |
taeyeobyeo | 97:908593afa823 | 113 | denoMINIRator = MAXIR[i] - MINIR[i]; |
taeyeobyeo | 97:908593afa823 | 114 | if(denoMINIRator != 0) x= (ir[i] - MINIR[i]) * 1000 /denoMINIRator; |
taeyeobyeo | 97:908593afa823 | 115 | if(x<0) x= 0; |
taeyeobyeo | 97:908593afa823 | 116 | else if ( x> 1000) x= 1000; |
taeyeobyeo | 97:908593afa823 | 117 | ir[i] = x; |
taeyeobyeo | 97:908593afa823 | 118 | } |
taeyeobyeo | 97:908593afa823 | 119 | } |
mbed_official | 88:bea4f2daa48c | 120 | |
taeyeobyeo | 97:908593afa823 | 121 | int readPosition(bool white){ |
taeyeobyeo | 97:908593afa823 | 122 | bool on_line = false; |
taeyeobyeo | 97:908593afa823 | 123 | int avg = 0, sum = 0; |
taeyeobyeo | 97:908593afa823 | 124 | readCalibrated(); |
taeyeobyeo | 97:908593afa823 | 125 | for(int i =0;i<5;i++){ |
taeyeobyeo | 97:908593afa823 | 126 | int value = ir[i]; |
taeyeobyeo | 97:908593afa823 | 127 | if(!white){ |
taeyeobyeo | 97:908593afa823 | 128 | value = 1000 - value; |
taeyeobyeo | 97:908593afa823 | 129 | ir[i] = value; |
taeyeobyeo | 97:908593afa823 | 130 | } |
taeyeobyeo | 97:908593afa823 | 131 | if(value > 300){ |
taeyeobyeo | 97:908593afa823 | 132 | on_line = true; |
taeyeobyeo | 97:908593afa823 | 133 | } |
taeyeobyeo | 97:908593afa823 | 134 | if(value > 50) { |
taeyeobyeo | 97:908593afa823 | 135 | avg += value * (i*1000); |
taeyeobyeo | 97:908593afa823 | 136 | sum += value; |
taeyeobyeo | 97:908593afa823 | 137 | } |
taeyeobyeo | 97:908593afa823 | 138 | } |
taeyeobyeo | 97:908593afa823 | 139 | if(!on_line){ |
taeyeobyeo | 97:908593afa823 | 140 | if(lastIR < 4000/2){ |
taeyeobyeo | 97:908593afa823 | 141 | return 0; |
taeyeobyeo | 97:908593afa823 | 142 | } |
taeyeobyeo | 97:908593afa823 | 143 | else return 4000; |
taeyeobyeo | 97:908593afa823 | 144 | } |
taeyeobyeo | 97:908593afa823 | 145 | lastIR = avg/sum; |
taeyeobyeo | 97:908593afa823 | 146 | return lastIR; |
taeyeobyeo | 97:908593afa823 | 147 | } |
taeyeobyeo | 97:908593afa823 | 148 | |
taeyeobyeo | 97:908593afa823 | 149 | void tick(){ |
taeyeobyeo | 97:908593afa823 | 150 | ultra.trig(); |
taeyeobyeo | 97:908593afa823 | 151 | // pc.printf("hello"); |
taeyeobyeo | 97:908593afa823 | 152 | } |
taeyeobyeo | 97:908593afa823 | 153 | |
taeyeobyeo | 97:908593afa823 | 154 | int last_proportional = 0; |
taeyeobyeo | 97:908593afa823 | 155 | int integral = 0; |
taeyeobyeo | 97:908593afa823 | 156 | const int maximum = 255; |
taeyeobyeo | 97:908593afa823 | 157 | const float maxspeed = 0.25; |
taeyeobyeo | 97:908593afa823 | 158 | const float radiant = 0.6; |
taeyeobyeo | 97:908593afa823 | 159 | float speed = 0.3; |
taeyeobyeo | 97:908593afa823 | 160 | float rightvar = 0.013; |
taeyeobyeo | 97:908593afa823 | 161 | |
taeyeobyeo | 97:908593afa823 | 162 | void printIR(){ |
taeyeobyeo | 97:908593afa823 | 163 | for (int i =0; i< 5; i++){ |
taeyeobyeo | 97:908593afa823 | 164 | pc.printf("%d: %d\r\n", i, ir[i]); |
taeyeobyeo | 97:908593afa823 | 165 | } |
taeyeobyeo | 97:908593afa823 | 166 | } |
taeyeobyeo | 97:908593afa823 | 167 | |
taeyeobyeo | 97:908593afa823 | 168 | |
mbed_official | 82:abf1b1785bd7 | 169 | int main() |
mbed_official | 82:abf1b1785bd7 | 170 | { |
taeyeobyeo | 97:908593afa823 | 171 | spi.format(16,0); |
taeyeobyeo | 97:908593afa823 | 172 | spi.frequency(2000000); |
taeyeobyeo | 97:908593afa823 | 173 | i2c.frequency(100000); |
taeyeobyeo | 97:908593afa823 | 174 | oled.begin(SSD1306_SWITCHCAPVCC); |
taeyeobyeo | 97:908593afa823 | 175 | oled.clearDisplay(); |
taeyeobyeo | 97:908593afa823 | 176 | timer.start(); |
taeyeobyeo | 97:908593afa823 | 177 | oled.printf("Calibration start\r"); |
taeyeobyeo | 97:908593afa823 | 178 | oled.display(); |
taeyeobyeo | 97:908593afa823 | 179 | bool calibration = true; |
taeyeobyeo | 97:908593afa823 | 180 | int start = 0, end = 0; |
taeyeobyeo | 97:908593afa823 | 181 | |
taeyeobyeo | 97:908593afa823 | 182 | while(true){ |
taeyeobyeo | 97:908593afa823 | 183 | if(irrecv.getState() == ReceiverIR::Received |
taeyeobyeo | 97:908593afa823 | 184 | && irrecv.getData(&format, buf, sizeof(buf)*8) != 0){ |
taeyeobyeo | 97:908593afa823 | 185 | switch(buf[3]){ |
taeyeobyeo | 97:908593afa823 | 186 | case 0xBA: |
taeyeobyeo | 97:908593afa823 | 187 | break; //ch -- |
taeyeobyeo | 97:908593afa823 | 188 | case 0xB9: break; //ch |
taeyeobyeo | 97:908593afa823 | 189 | case 0xBB: |
taeyeobyeo | 97:908593afa823 | 190 | rightvar -= 0.05; |
taeyeobyeo | 97:908593afa823 | 191 | if(rightvar < -2.0) rightvar = -2.0; |
taeyeobyeo | 97:908593afa823 | 192 | break; //<< |
taeyeobyeo | 97:908593afa823 | 193 | case 0xBF: |
taeyeobyeo | 97:908593afa823 | 194 | rightvar += 0.05; |
taeyeobyeo | 97:908593afa823 | 195 | if(rightvar > 2.0) rightvar = 2.0; |
taeyeobyeo | 97:908593afa823 | 196 | break; //>> |
taeyeobyeo | 97:908593afa823 | 197 | case 0xBC: calibration = false; break; //>|| |
taeyeobyeo | 97:908593afa823 | 198 | case 0xF8: |
taeyeobyeo | 97:908593afa823 | 199 | speed -= 0.1; |
taeyeobyeo | 97:908593afa823 | 200 | if( speed < -1.0) speed = -1.0; |
taeyeobyeo | 97:908593afa823 | 201 | break; // - |
taeyeobyeo | 97:908593afa823 | 202 | case 0xEA: |
taeyeobyeo | 97:908593afa823 | 203 | speed += 0.1; |
taeyeobyeo | 97:908593afa823 | 204 | if( speed >1.0) speed = 1.0; |
taeyeobyeo | 97:908593afa823 | 205 | break; // + |
taeyeobyeo | 97:908593afa823 | 206 | case 0xF6: |
taeyeobyeo | 97:908593afa823 | 207 | //left.speed(0.2); |
taeyeobyeo | 97:908593afa823 | 208 | // right.speed(-0.2); |
taeyeobyeo | 97:908593afa823 | 209 | oled.clearDisplay(); |
taeyeobyeo | 97:908593afa823 | 210 | oled.printf("Calibrating\r"); |
taeyeobyeo | 97:908593afa823 | 211 | oled.display(); |
taeyeobyeo | 97:908593afa823 | 212 | for (int i =0; i< 100; i++) |
taeyeobyeo | 97:908593afa823 | 213 | calibrate(); |
taeyeobyeo | 97:908593afa823 | 214 | oled.clearDisplay(); |
taeyeobyeo | 97:908593afa823 | 215 | oled.printf("Calibrated!!\r"); |
taeyeobyeo | 97:908593afa823 | 216 | oled.display(); |
taeyeobyeo | 97:908593afa823 | 217 | for(int i =0; i<5;i++){ |
taeyeobyeo | 97:908593afa823 | 218 | pc.printf("MIN: %d, MAX: %d\r\n", MINIR[i], MAXIR[i]); |
taeyeobyeo | 97:908593afa823 | 219 | } |
taeyeobyeo | 97:908593afa823 | 220 | // wait(0.7); |
taeyeobyeo | 97:908593afa823 | 221 | // left.speed(0.0); |
taeyeobyeo | 97:908593afa823 | 222 | // right.speed(0.0); |
taeyeobyeo | 97:908593afa823 | 223 | // pc.printf("\r\ncalibrated\r\n"); |
taeyeobyeo | 97:908593afa823 | 224 | break; //EQ |
taeyeobyeo | 97:908593afa823 | 225 | case 0xE6: break; //100+ |
taeyeobyeo | 97:908593afa823 | 226 | case 0xF2: break; //200+ |
taeyeobyeo | 97:908593afa823 | 227 | case 0xE9: break; //0 |
taeyeobyeo | 97:908593afa823 | 228 | case 0xF3: break; //1 |
taeyeobyeo | 97:908593afa823 | 229 | case 0xE7: |
taeyeobyeo | 97:908593afa823 | 230 | left.speed(speed); |
taeyeobyeo | 97:908593afa823 | 231 | right.speed(speed + rightvar); |
taeyeobyeo | 97:908593afa823 | 232 | break; //2 |
taeyeobyeo | 97:908593afa823 | 233 | case 0xA1: break; //3 |
taeyeobyeo | 97:908593afa823 | 234 | case 0xF7: |
taeyeobyeo | 97:908593afa823 | 235 | left.speed(speed/2); |
taeyeobyeo | 97:908593afa823 | 236 | right.speed(speed + rightvar); |
taeyeobyeo | 97:908593afa823 | 237 | break; //4 |
taeyeobyeo | 97:908593afa823 | 238 | case 0xE3: |
taeyeobyeo | 97:908593afa823 | 239 | left.speed(0); |
taeyeobyeo | 97:908593afa823 | 240 | right.speed(0); |
taeyeobyeo | 97:908593afa823 | 241 | break; //5 |
taeyeobyeo | 97:908593afa823 | 242 | case 0xA5: |
taeyeobyeo | 97:908593afa823 | 243 | left.speed(speed); |
taeyeobyeo | 97:908593afa823 | 244 | right.speed(speed/2 + rightvar); |
taeyeobyeo | 97:908593afa823 | 245 | break; //6 |
taeyeobyeo | 97:908593afa823 | 246 | case 0xBD: break; //7 |
taeyeobyeo | 97:908593afa823 | 247 | case 0xAD: |
taeyeobyeo | 97:908593afa823 | 248 | left.speed(-speed); |
taeyeobyeo | 97:908593afa823 | 249 | right.speed(-(speed + rightvar)); |
taeyeobyeo | 97:908593afa823 | 250 | break; //8 |
taeyeobyeo | 97:908593afa823 | 251 | case 0xB5: break; //9 |
taeyeobyeo | 97:908593afa823 | 252 | default:break; |
taeyeobyeo | 97:908593afa823 | 253 | } |
taeyeobyeo | 97:908593afa823 | 254 | } |
taeyeobyeo | 97:908593afa823 | 255 | if(!calibration) break; |
taeyeobyeo | 97:908593afa823 | 256 | } |
taeyeobyeo | 97:908593afa823 | 257 | oled.clearDisplay(); |
taeyeobyeo | 97:908593afa823 | 258 | oled.printf("Start!\r"); |
taeyeobyeo | 97:908593afa823 | 259 | oled.display(); |
taeyeobyeo | 97:908593afa823 | 260 | |
taeyeobyeo | 97:908593afa823 | 261 | // ticker.attach(&tick, 1.0); |
mbed_official | 82:abf1b1785bd7 | 262 | |
taeyeobyeo | 97:908593afa823 | 263 | start = timer.read_us(); |
taeyeobyeo | 97:908593afa823 | 264 | while (true) { |
taeyeobyeo | 97:908593afa823 | 265 | ultra.trig(); |
taeyeobyeo | 97:908593afa823 | 266 | if(ultra.getStatus() == 1) { |
taeyeobyeo | 97:908593afa823 | 267 | int distance = ultra.getDistance(); |
taeyeobyeo | 97:908593afa823 | 268 | if(distance >18 && distance <25){ |
taeyeobyeo | 97:908593afa823 | 269 | left.speed(-0.4); |
taeyeobyeo | 97:908593afa823 | 270 | right.speed(0.4); |
taeyeobyeo | 97:908593afa823 | 271 | wait(0.15); |
taeyeobyeo | 97:908593afa823 | 272 | left.speed(0.0); |
taeyeobyeo | 97:908593afa823 | 273 | right.speed(0.0); |
taeyeobyeo | 97:908593afa823 | 274 | integral = 0; |
taeyeobyeo | 97:908593afa823 | 275 | last_proportional = 0; |
taeyeobyeo | 97:908593afa823 | 276 | } |
taeyeobyeo | 97:908593afa823 | 277 | ultra.clearStatus(); |
taeyeobyeo | 97:908593afa823 | 278 | } |
taeyeobyeo | 97:908593afa823 | 279 | int position = readPosition(false); //ir1, ir2, ir3, ir4, ir5에 값이 들어감 |
taeyeobyeo | 97:908593afa823 | 280 | int proportional = (int)position - 2000; |
taeyeobyeo | 97:908593afa823 | 281 | int derivative = proportional - last_proportional; |
taeyeobyeo | 97:908593afa823 | 282 | integral += proportional; |
taeyeobyeo | 97:908593afa823 | 283 | last_proportional = proportional; |
taeyeobyeo | 97:908593afa823 | 284 | int power_difference = proportional/20 + integral/10000 + derivative*10; |
taeyeobyeo | 97:908593afa823 | 285 | |
taeyeobyeo | 97:908593afa823 | 286 | if (power_difference > maximum) |
taeyeobyeo | 97:908593afa823 | 287 | power_difference = maximum; |
taeyeobyeo | 97:908593afa823 | 288 | if (power_difference < -maximum) |
taeyeobyeo | 97:908593afa823 | 289 | power_difference = -maximum; |
taeyeobyeo | 97:908593afa823 | 290 | // pc.printf("position: %d proportion: %d last_proportional: %d pd: %d\r\n", position, proportional, last_proportional, power_difference); |
taeyeobyeo | 97:908593afa823 | 291 | if (power_difference < 0) |
taeyeobyeo | 97:908593afa823 | 292 | { |
taeyeobyeo | 97:908593afa823 | 293 | left.speed((maxspeed + (float)power_difference / maximum * maxspeed)); |
taeyeobyeo | 97:908593afa823 | 294 | right.speed(maxspeed); |
taeyeobyeo | 97:908593afa823 | 295 | // pc.printf("%f\r\n",maxspeed + (float)power_difference * maxspeed *radiant / maximum); |
mbed_official | 88:bea4f2daa48c | 296 | } |
taeyeobyeo | 97:908593afa823 | 297 | else |
taeyeobyeo | 97:908593afa823 | 298 | { |
taeyeobyeo | 97:908593afa823 | 299 | left.speed(maxspeed); |
taeyeobyeo | 97:908593afa823 | 300 | right.speed((maxspeed - (float)power_difference / maximum * maxspeed)); |
taeyeobyeo | 97:908593afa823 | 301 | // pc.printf("%f\r\n",maxspeed - (float)power_difference * maxspeed *radiant / maximum); |
taeyeobyeo | 97:908593afa823 | 302 | } |
taeyeobyeo | 97:908593afa823 | 303 | |
taeyeobyeo | 97:908593afa823 | 304 | if (ir[1] > 800 && ir[2] > 800 && ir[3] > 800) |
taeyeobyeo | 97:908593afa823 | 305 | { |
taeyeobyeo | 97:908593afa823 | 306 | left.speed(0.0); |
taeyeobyeo | 97:908593afa823 | 307 | right.speed(0.0); |
taeyeobyeo | 97:908593afa823 | 308 | end = timer.read_us(); |
taeyeobyeo | 97:908593afa823 | 309 | oled.clearDisplay(); |
taeyeobyeo | 97:908593afa823 | 310 | oled.printf("lap: %d ms\r", (end - start)/1000); |
taeyeobyeo | 97:908593afa823 | 311 | oled.display(); |
taeyeobyeo | 97:908593afa823 | 312 | wsInit(); |
taeyeobyeo | 97:908593afa823 | 313 | ws.write_offsets(px.getBuf(),WS2812_BUF,WS2812_BUF,WS2812_BUF); |
taeyeobyeo | 97:908593afa823 | 314 | // pc.printf("%d\r\n", end - start); |
taeyeobyeo | 97:908593afa823 | 315 | wait(15.0); |
taeyeobyeo | 97:908593afa823 | 316 | } |
taeyeobyeo | 97:908593afa823 | 317 | wait(0.05); |
taeyeobyeo | 97:908593afa823 | 318 | // left.speed(0.0); |
taeyeobyeo | 97:908593afa823 | 319 | // right.speed(0.0); |
taeyeobyeo | 97:908593afa823 | 320 | //// pc.printf("\r\n"); |
taeyeobyeo | 97:908593afa823 | 321 | //// printIR(); |
taeyeobyeo | 97:908593afa823 | 322 | // pc.printf("pd: %d\r\n",power_difference); |
taeyeobyeo | 97:908593afa823 | 323 | // pc.printf("p: %d\r\n",last_proportional/20); |
taeyeobyeo | 97:908593afa823 | 324 | // pc.printf("p: %d\r\n",proportional/20); |
taeyeobyeo | 97:908593afa823 | 325 | // pc.printf("i: %d\r\n",integral/10000); |
taeyeobyeo | 97:908593afa823 | 326 | // pc.printf("d: %d\r\n",derivative/20); |
taeyeobyeo | 97:908593afa823 | 327 | // pc.getc(); |
Jonathan Austin |
0:2757d7abb7d9 | 328 | } |
taeyeobyeo | 97:908593afa823 | 329 | } |