Alphabot_Group2 / Mbed OS Group2

Alphabot

Dependencies:   WS2812 PixelArray Adafruit_GFX HC_SR04_Ultrasonic_Library

Fork of 15_FJ_1 by Minsu Kim

main.cpp@99:6efb1c0a6a68, 2019-06-17 (annotated)

Committer:
kmsmile2
Date:
Mon Jun 17 02:28:58 2019 +0000
Revision:
99:6efb1c0a6a68
Parent:
98:d8ead5e04047 
2_final

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kmsmile2 97:8f3abd5cf5ce 1 #include "mbed.h"
kmsmile2 97:8f3abd5cf5ce 2 #include "RemoteIR.h"
kmsmile2 97:8f3abd5cf5ce 3 #include "ReceiverIR.h"
kmsmile2 97:8f3abd5cf5ce 4 #include "TB6612FNG.h"
kmsmile2 97:8f3abd5cf5ce 5 #include "TRSensors.h"
kmsmile2 97:8f3abd5cf5ce 6 #include "ultrasonic.h"
kmsmile2 97:8f3abd5cf5ce 7 #include "Adafruit_SSD1306.h"
kmsmile2 98:d8ead5e04047 8 #include "WS2812.h"
kmsmile2 98:d8ead5e04047 9 #include "PixelArray.h"
kmsmile2 97:8f3abd5cf5ce 10
kmsmile2 97:8f3abd5cf5ce 11 #define button_SENSORS 5
kmsmile2 97:8f3abd5cf5ce 12 #define ADT7420_TEMP_REG (0x00)
kmsmile2 97:8f3abd5cf5ce 13 #define ADT7420_CONF_REG (0x03)
kmsmile2 97:8f3abd5cf5ce 14 #define EVAL_ADT7420_ADDR (0x48)
kmsmile2 97:8f3abd5cf5ce 15 #define PCF8574_ADDR (0x20)
kmsmile2 98:d8ead5e04047 16 #define WS2812_BUF 100
kmsmile2 98:d8ead5e04047 17 #define WS2812_BUF2 4
kmsmile2 98:d8ead5e04047 18 #define NUM_COLORS 3
kmsmile2 98:d8ead5e04047 19 #define NUM_LEDS_PER_COLOR 4
mbed_official 82:abf1b1785bd7 20
kmsmile2 97:8f3abd5cf5ce 21 // create object
kmsmile2 97:8f3abd5cf5ce 22 DigitalOut dc(D8,1);
kmsmile2 97:8f3abd5cf5ce 23 DigitalOut rst(D9,1);
kmsmile2 97:8f3abd5cf5ce 24 I2C i2c(I2C_SDA, I2C_SCL);
kmsmile2 97:8f3abd5cf5ce 25 TB6612FNG motorDriver(D6, A1, A0, D5, A2, A3);
kmsmile2 97:8f3abd5cf5ce 26 Ultrasonic ultra(D3, D2, .1, false);
kmsmile2 97:8f3abd5cf5ce 27 ReceiverIR IR(D4);
kmsmile2 97:8f3abd5cf5ce 28 TRSensors trs;
kmsmile2 97:8f3abd5cf5ce 29 RawSerial pc(USBTX, USBRX, 115200);
kmsmile2 97:8f3abd5cf5ce 30 Adafruit_SSD1306_I2c gOled2(i2c,D9,0x7A,64,128);
kmsmile2 97:8f3abd5cf5ce 31 Ticker IRticker;
kmsmile2 97:8f3abd5cf5ce 32 Thread thread1;
kmsmile2 97:8f3abd5cf5ce 33 Thread thread2;
kmsmile2 97:8f3abd5cf5ce 34 Timer timer;
kmsmile2 98:d8ead5e04047 35 PixelArray px(WS2812_BUF);
kmsmile2 98:d8ead5e04047 36
kmsmile2 98:d8ead5e04047 37 // See the program page for information on the timing numbers
kmsmile2 98:d8ead5e04047 38 // The given numbers are for the K64F
kmsmile2 98:d8ead5e04047 39 WS2812 ws(D7, WS2812_BUF, 7, 15, 10, 15);
mbed_official 88:bea4f2daa48c 40
kmsmile2 97:8f3abd5cf5ce 41 // variables
kmsmile2 97:8f3abd5cf5ce 42 unsigned int sensorValues[button_SENSORS];
kmsmile2 97:8f3abd5cf5ce 43 unsigned int last_proportional = 0;
kmsmile2 97:8f3abd5cf5ce 44 long integral = 0;
kmsmile2 97:8f3abd5cf5ce 45 static int prev_err = 0;
kmsmile2 97:8f3abd5cf5ce 46 static int int_err = 0;
kmsmile2 98:d8ead5e04047 47 static float pval = 0.15;
kmsmile2 98:d8ead5e04047 48 static float ival = 0.00003;
kmsmile2 98:d8ead5e04047 49 static float dval = 2.3;
kmsmile2 98:d8ead5e04047 50 static float vel = 120.0;
kmsmile2 97:8f3abd5cf5ce 51 uint8_t IR_buf[64];
kmsmile2 97:8f3abd5cf5ce 52 int length = 32;
kmsmile2 97:8f3abd5cf5ce 53 volatile int button = 0;
kmsmile2 97:8f3abd5cf5ce 54 int position = 0;
kmsmile2 97:8f3abd5cf5ce 55 int dist = 0;
kmsmile2 97:8f3abd5cf5ce 56 int cnt = 0;
kmsmile2 97:8f3abd5cf5ce 57 int err = 0;
kmsmile2 97:8f3abd5cf5ce 58 float t=0;
mbed_official 82:abf1b1785bd7 59
kmsmile2 97:8f3abd5cf5ce 60 // Reflective sensor
kmsmile2 97:8f3abd5cf5ce 61 bool rightObs= false;
kmsmile2 97:8f3abd5cf5ce 62 bool leftObs = false;
kmsmile2 97:8f3abd5cf5ce 63
kmsmile2 97:8f3abd5cf5ce 64 // motor Driver
kmsmile2 97:8f3abd5cf5ce 65 float fPwmAPeriod;
kmsmile2 97:8f3abd5cf5ce 66 float fPwmAPulsewidth;
kmsmile2 97:8f3abd5cf5ce 67 float fPwmBPeriod;
kmsmile2 97:8f3abd5cf5ce 68 float fPwmBPulsewidth;
mbed_official 82:abf1b1785bd7 69
kmsmile2 97:8f3abd5cf5ce 70 // opearte ultrasonic
kmsmile2 98:d8ead5e04047 71 //void ultrasonic_thread()
kmsmile2 98:d8ead5e04047 72 //{
kmsmile2 98:d8ead5e04047 73 // while (1)
kmsmile2 98:d8ead5e04047 74 // {
kmsmile2 98:d8ead5e04047 75 // ultra.setMode(false); // have updates every .1 seconds and try only once.
kmsmile2 98:d8ead5e04047 76 // ultra.trig();
kmsmile2 98:d8ead5e04047 77 // dist = ultra.getDistance();
kmsmile2 98:d8ead5e04047 78 // pc.printf("Detected value: %d\r\n", dist);
kmsmile2 98:d8ead5e04047 79 // }
kmsmile2 98:d8ead5e04047 80 //}
kmsmile2 98:d8ead5e04047 81
kmsmile2 97:8f3abd5cf5ce 82
kmsmile2 97:8f3abd5cf5ce 83 // opearte reflective sensors
kmsmile2 97:8f3abd5cf5ce 84 /*
kmsmile2 97:8f3abd5cf5ce 85 void reflective_thread()
kmsmile2 97:8f3abd5cf5ce 86 {
kmsmile2 97:8f3abd5cf5ce 87 while(1){
kmsmile2 97:8f3abd5cf5ce 88 char data_write[2];
kmsmile2 97:8f3abd5cf5ce 89 char data_read[2];
kmsmile2 97:8f3abd5cf5ce 90 i2c.frequency(100000);
kmsmile2 97:8f3abd5cf5ce 91 i2c.read((PCF8574_ADDR<<1|0x01), data_read, 2, 0);
kmsmile2 97:8f3abd5cf5ce 92 pc.printf("hexa: %x, %x\r\n", data_read[0], data_read[1]);
kmsmile2 97:8f3abd5cf5ce 93 if(data_read[0]>>6 == 0b10)
kmsmile2 97:8f3abd5cf5ce 94 {
kmsmile2 97:8f3abd5cf5ce 95 pc.printf("Obstacle on left!\r\n");
kmsmile2 97:8f3abd5cf5ce 96 data_write[0] = 0xDF;
kmsmile2 97:8f3abd5cf5ce 97 leftObs = true;
kmsmile2 97:8f3abd5cf5ce 98 }
kmsmile2 97:8f3abd5cf5ce 99 else if(data_read[0]>>6 == 0b01)
kmsmile2 97:8f3abd5cf5ce 100 {
kmsmile2 97:8f3abd5cf5ce 101 pc.printf("Obstacle on right!\r\n");
kmsmile2 97:8f3abd5cf5ce 102 data_write[0] = 0xDF;
kmsmile2 97:8f3abd5cf5ce 103 rightObs = true;
kmsmile2 97:8f3abd5cf5ce 104 }
kmsmile2 97:8f3abd5cf5ce 105 else if(data_read[0]>>6 == 0b00)
kmsmile2 97:8f3abd5cf5ce 106 {
kmsmile2 97:8f3abd5cf5ce 107 pc.printf("Obstacle on front!\r\n");
kmsmile2 97:8f3abd5cf5ce 108 data_write[0] = 0xDF;
kmsmile2 97:8f3abd5cf5ce 109 leftObs = true;
kmsmile2 97:8f3abd5cf5ce 110 rightObs = true;
kmsmile2 97:8f3abd5cf5ce 111 wait(3);
kmsmile2 97:8f3abd5cf5ce 112 }
kmsmile2 97:8f3abd5cf5ce 113
kmsmile2 97:8f3abd5cf5ce 114 else
kmsmile2 97:8f3abd5cf5ce 115 {
kmsmile2 97:8f3abd5cf5ce 116 data_write[0]=0xFF;
kmsmile2 97:8f3abd5cf5ce 117 }
kmsmile2 97:8f3abd5cf5ce 118 i2c.write((PCF8574_ADDR<<1),data_write,1,0);
kmsmile2 97:8f3abd5cf5ce 119 }
kmsmile2 97:8f3abd5cf5ce 120 }*/
kmsmile2 97:8f3abd5cf5ce 121
kmsmile2 97:8f3abd5cf5ce 122 inline void update_display(){
kmsmile2 97:8f3abd5cf5ce 123 gOled2.clearDisplay();
kmsmile2 97:8f3abd5cf5ce 124 gOled2.setTextCursor(0,0);
kmsmile2 97:8f3abd5cf5ce 125 gOled2.printf("E-RON alphabot\r\n");
kmsmile2 97:8f3abd5cf5ce 126 gOled2.printf("Timer: %.2f\r\n",t);
kmsmile2 97:8f3abd5cf5ce 127 gOled2.printf("V: %.0f\r\n",vel);
kmsmile2 97:8f3abd5cf5ce 128 gOled2.printf("P: %.2f\r\n",pval);
kmsmile2 97:8f3abd5cf5ce 129 gOled2.printf("I: %.5f\r\n",ival);
kmsmile2 97:8f3abd5cf5ce 130 gOled2.printf("D: %.2f\r\n",dval);
kmsmile2 97:8f3abd5cf5ce 131 gOled2.display();
kmsmile2 97:8f3abd5cf5ce 132 }
kmsmile2 98:d8ead5e04047 133 int colorbuf[NUM_COLORS] = {0xff0000,0x00ff00,0x0000ff};
kmsmile2 98:d8ead5e04047 134 int colorbuf3 =0x000000;
kmsmile2 97:8f3abd5cf5ce 135 int main()
kmsmile2 98:d8ead5e04047 136 {
kmsmile2 98:d8ead5e04047 137 for (int i = 0; i < WS2812_BUF; i++) {
kmsmile2 98:d8ead5e04047 138 px.Set(i, colorbuf[(i / NUM_LEDS_PER_COLOR) % NUM_COLORS]);
kmsmile2 98:d8ead5e04047 139 }
kmsmile2 98:d8ead5e04047 140 for (int j=0; j<WS2812_BUF; j++) {
kmsmile2 98:d8ead5e04047 141 // px.SetI(pixel position, II value)
kmsmile2 98:d8ead5e04047 142 px.SetI(j%WS2812_BUF, 0xf+(0xf*4));
kmsmile2 98:d8ead5e04047 143 }
kmsmile2 97:8f3abd5cf5ce 144
kmsmile2 97:8f3abd5cf5ce 145 // thread1.start(ultrasonic_thread);
kmsmile2 97:8f3abd5cf5ce 146 // thread2.start(reflective_thread);
kmsmile2 97:8f3abd5cf5ce 147 RemoteIR::Format format;
kmsmile2 97:8f3abd5cf5ce 148 uint8_t buf[32];
kmsmile2 97:8f3abd5cf5ce 149 int bitcount;
kmsmile2 97:8f3abd5cf5ce 150 update_display();
kmsmile2 97:8f3abd5cf5ce 151 while(1)
kmsmile2 97:8f3abd5cf5ce 152 { // read the value of the code
kmsmile2 97:8f3abd5cf5ce 153 if (IR.getState() == ReceiverIR::Received)
kmsmile2 97:8f3abd5cf5ce 154 {
kmsmile2 97:8f3abd5cf5ce 155 bitcount = IR.getData(&format, buf, sizeof(buf) * 8);
kmsmile2 97:8f3abd5cf5ce 156 pc.printf("%d\r\n", bitcount);
kmsmile2 97:8f3abd5cf5ce 157 pc.printf("buf[2]: %d\r\n", buf[2]);
kmsmile2 97:8f3abd5cf5ce 158 button = buf[2];
kmsmile2 97:8f3abd5cf5ce 159 }
kmsmile2 97:8f3abd5cf5ce 160
kmsmile2 97:8f3abd5cf5ce 161 switch(button)
kmsmile2 97:8f3abd5cf5ce 162 {
kmsmile2 97:8f3abd5cf5ce 163 case 0x45:
kmsmile2 97:8f3abd5cf5ce 164 // ch- button (P value up)
kmsmile2 97:8f3abd5cf5ce 165 button = 0x1C;
kmsmile2 97:8f3abd5cf5ce 166 pval += 0.01;
kmsmile2 97:8f3abd5cf5ce 167 pval = fabs(pval);
kmsmile2 97:8f3abd5cf5ce 168 update_display();
kmsmile2 97:8f3abd5cf5ce 169 // pc.printf("pval:%f\r\n", pval);
kmsmile2 97:8f3abd5cf5ce 170 wait(0.3);
kmsmile2 97:8f3abd5cf5ce 171 break;
kmsmile2 97:8f3abd5cf5ce 172
kmsmile2 97:8f3abd5cf5ce 173 case 0x46:
kmsmile2 97:8f3abd5cf5ce 174 // ch common button (I value up)
kmsmile2 97:8f3abd5cf5ce 175 button = 0x1C;
kmsmile2 97:8f3abd5cf5ce 176 ival += 0.00001;
kmsmile2 97:8f3abd5cf5ce 177 ival = fabs(ival);
kmsmile2 97:8f3abd5cf5ce 178 update_display() ;
kmsmile2 97:8f3abd5cf5ce 179 // pc.printf("ival:%f\r\n", ival);
kmsmile2 97:8f3abd5cf5ce 180 wait(0.3);
kmsmile2 97:8f3abd5cf5ce 181 break;
kmsmile2 97:8f3abd5cf5ce 182
kmsmile2 97:8f3abd5cf5ce 183 case 0x47:
kmsmile2 97:8f3abd5cf5ce 184 // ch+ button (D value up)
kmsmile2 97:8f3abd5cf5ce 185 button = 0x1C;
kmsmile2 97:8f3abd5cf5ce 186 dval += 0.05;
kmsmile2 97:8f3abd5cf5ce 187 dval = fabs(dval);
kmsmile2 97:8f3abd5cf5ce 188 update_display();
kmsmile2 97:8f3abd5cf5ce 189 wait(0.3);
kmsmile2 97:8f3abd5cf5ce 190 break;
kmsmile2 97:8f3abd5cf5ce 191
kmsmile2 97:8f3abd5cf5ce 192 case 0x44:
kmsmile2 97:8f3abd5cf5ce 193 // prev button (P value down)
kmsmile2 97:8f3abd5cf5ce 194 button = 0x1C;
kmsmile2 97:8f3abd5cf5ce 195 pval -= 0.01;
kmsmile2 97:8f3abd5cf5ce 196 pval = fabs(pval);
kmsmile2 97:8f3abd5cf5ce 197 update_display();
kmsmile2 97:8f3abd5cf5ce 198 wait(0.3);
kmsmile2 97:8f3abd5cf5ce 199 break;
kmsmile2 97:8f3abd5cf5ce 200
kmsmile2 97:8f3abd5cf5ce 201 case 0x40:
kmsmile2 97:8f3abd5cf5ce 202 // next button (I value down)
kmsmile2 97:8f3abd5cf5ce 203 button = 0x1C;
kmsmile2 97:8f3abd5cf5ce 204 ival -= 0.00001;
kmsmile2 97:8f3abd5cf5ce 205 ival = fabs(ival);
kmsmile2 97:8f3abd5cf5ce 206 update_display();
kmsmile2 97:8f3abd5cf5ce 207 wait(0.3);
kmsmile2 97:8f3abd5cf5ce 208 button = 0x1C;
kmsmile2 97:8f3abd5cf5ce 209 break;
kmsmile2 97:8f3abd5cf5ce 210
kmsmile2 97:8f3abd5cf5ce 211 case 0x43:
kmsmile2 97:8f3abd5cf5ce 212 // play/pause button (D value down)
kmsmile2 97:8f3abd5cf5ce 213 button = 0x1C;
kmsmile2 97:8f3abd5cf5ce 214 dval -= 0.05;
kmsmile2 97:8f3abd5cf5ce 215 dval = fabs(dval);
kmsmile2 97:8f3abd5cf5ce 216 update_display();
kmsmile2 97:8f3abd5cf5ce 217 wait(0.3);
kmsmile2 97:8f3abd5cf5ce 218 button = 0x1C;
kmsmile2 97:8f3abd5cf5ce 219 break;
kmsmile2 97:8f3abd5cf5ce 220
kmsmile2 97:8f3abd5cf5ce 221 case 0x07:
kmsmile2 97:8f3abd5cf5ce 222 // vol- button (velocity down)
kmsmile2 97:8f3abd5cf5ce 223 button = 0x1C;
kmsmile2 97:8f3abd5cf5ce 224 vel -= 5;
kmsmile2 97:8f3abd5cf5ce 225 update_display();
kmsmile2 97:8f3abd5cf5ce 226 wait(0.3);
kmsmile2 97:8f3abd5cf5ce 227 button = 0x1C;
kmsmile2 97:8f3abd5cf5ce 228 break;
kmsmile2 97:8f3abd5cf5ce 229
kmsmile2 97:8f3abd5cf5ce 230 case 0x15:
kmsmile2 97:8f3abd5cf5ce 231 // vol+ button (velocity up)
kmsmile2 97:8f3abd5cf5ce 232 button = 0x1C;
kmsmile2 97:8f3abd5cf5ce 233 vel += 5;
kmsmile2 97:8f3abd5cf5ce 234 update_display();
kmsmile2 97:8f3abd5cf5ce 235 wait(0.3);
kmsmile2 97:8f3abd5cf5ce 236 button = 0x1C;
kmsmile2 97:8f3abd5cf5ce 237 break;
kmsmile2 97:8f3abd5cf5ce 238
kmsmile2 97:8f3abd5cf5ce 239 case 0x16:
kmsmile2 97:8f3abd5cf5ce 240 // 0 button (Reset the error variables)
kmsmile2 97:8f3abd5cf5ce 241 int_err = 0;
kmsmile2 97:8f3abd5cf5ce 242 err = 0;
kmsmile2 97:8f3abd5cf5ce 243 prev_err = 0;
kmsmile2 97:8f3abd5cf5ce 244 wait(1);
kmsmile2 97:8f3abd5cf5ce 245 button = 0x1C;
kmsmile2 97:8f3abd5cf5ce 246 break;
kmsmile2 97:8f3abd5cf5ce 247
kmsmile2 97:8f3abd5cf5ce 248 case 0x19:
kmsmile2 99:6efb1c0a6a68 249 // 100+ button (Calibrate)
kmsmile2 99:6efb1c0a6a68 250 motorDriver.user_left(0.2,0.2);
kmsmile2 97:8f3abd5cf5ce 251 pc.printf("calibrate!\r\n");
kmsmile2 97:8f3abd5cf5ce 252 trs.calibrate();
kmsmile2 97:8f3abd5cf5ce 253 pc.printf("calibrate done\r\n");
kmsmile2 97:8f3abd5cf5ce 254 wait(0.2);
kmsmile2 99:6efb1c0a6a68 255 //button = 0x1C;
kmsmile2 97:8f3abd5cf5ce 256 break;
kmsmile2 97:8f3abd5cf5ce 257
kmsmile2 97:8f3abd5cf5ce 258 case 0x18:
kmsmile2 97:8f3abd5cf5ce 259 // 2 button (move forward)
kmsmile2 97:8f3abd5cf5ce 260 motorDriver.user_forward(0.3,0.3);
kmsmile2 98:d8ead5e04047 261 trs.calibrate();
kmsmile2 97:8f3abd5cf5ce 262 wait(0.1);
kmsmile2 97:8f3abd5cf5ce 263 button = 0x1C;
kmsmile2 97:8f3abd5cf5ce 264 break;
kmsmile2 97:8f3abd5cf5ce 265
kmsmile2 97:8f3abd5cf5ce 266 case 0x52:
kmsmile2 97:8f3abd5cf5ce 267 // 8 button (move backward)
kmsmile2 97:8f3abd5cf5ce 268 motorDriver.user_backward(0.3,0.3);
kmsmile2 98:d8ead5e04047 269 trs.calibrate();
kmsmile2 97:8f3abd5cf5ce 270 wait(0.1);
kmsmile2 97:8f3abd5cf5ce 271 button = 0x1C;
kmsmile2 97:8f3abd5cf5ce 272 break;
kmsmile2 97:8f3abd5cf5ce 273
kmsmile2 97:8f3abd5cf5ce 274 case 0x5A:
kmsmile2 97:8f3abd5cf5ce 275 // 6 button (clockwise turn)
kmsmile2 99:6efb1c0a6a68 276 motorDriver.right();
kmsmile2 97:8f3abd5cf5ce 277 wait(0.1);
kmsmile2 97:8f3abd5cf5ce 278 button = 0x1C;
kmsmile2 97:8f3abd5cf5ce 279 break;
kmsmile2 97:8f3abd5cf5ce 280
kmsmile2 97:8f3abd5cf5ce 281 case 0x08:
kmsmile2 97:8f3abd5cf5ce 282 // 4 button (counter clockwise turn)
kmsmile2 99:6efb1c0a6a68 283 motorDriver.left();
kmsmile2 97:8f3abd5cf5ce 284 wait(0.1);
kmsmile2 97:8f3abd5cf5ce 285 button = 0x1C;
kmsmile2 97:8f3abd5cf5ce 286 break;
kmsmile2 97:8f3abd5cf5ce 287
kmsmile2 97:8f3abd5cf5ce 288 case 0x1C:
kmsmile2 97:8f3abd5cf5ce 289 // 5 button (motor stop)
kmsmile2 97:8f3abd5cf5ce 290 motorDriver.stop();
kmsmile2 97:8f3abd5cf5ce 291 break;
kmsmile2 97:8f3abd5cf5ce 292
kmsmile2 97:8f3abd5cf5ce 293 case 0x0D:
kmsmile2 97:8f3abd5cf5ce 294 // 200+ button (line tracer)
kmsmile2 97:8f3abd5cf5ce 295 t=0;
kmsmile2 97:8f3abd5cf5ce 296 timer.reset();
kmsmile2 97:8f3abd5cf5ce 297 timer.start();
kmsmile2 98:d8ead5e04047 298 ultra.setMode(false); // have updates every .1 seconds and try only once.
kmsmile2 99:6efb1c0a6a68 299 int_err = 0;
kmsmile2 99:6efb1c0a6a68 300 err = 0;
kmsmile2 99:6efb1c0a6a68 301 prev_err = 0;
kmsmile2 97:8f3abd5cf5ce 302 while(1)
kmsmile2 97:8f3abd5cf5ce 303 {
kmsmile2 97:8f3abd5cf5ce 304 t=timer.read();
kmsmile2 97:8f3abd5cf5ce 305 position=trs.readLine(sensorValues,0);
kmsmile2 98:d8ead5e04047 306 ultra.trig();
kmsmile2 98:d8ead5e04047 307 dist = ultra.getDistance();
kmsmile2 99:6efb1c0a6a68 308 // pc.printf("dist:%d\r\n", dist);
kmsmile2 98:d8ead5e04047 309 if(dist<=21){
kmsmile2 98:d8ead5e04047 310 motorDriver.user_left(0.2,0.2);
kmsmile2 98:d8ead5e04047 311 wait(0.1);
kmsmile2 98:d8ead5e04047 312 while(1){
kmsmile2 98:d8ead5e04047 313
kmsmile2 98:d8ead5e04047 314 // pc.printf("distance: %d\r\n", dist);
kmsmile2 98:d8ead5e04047 315 // pc.printf("position: %d\r\n", position);
kmsmile2 97:8f3abd5cf5ce 316 position=trs.readLine(sensorValues,0);
kmsmile2 97:8f3abd5cf5ce 317 if(position > 2000)
kmsmile2 97:8f3abd5cf5ce 318 {
kmsmile2 97:8f3abd5cf5ce 319 int_err = 0;
kmsmile2 97:8f3abd5cf5ce 320 err = 0;
kmsmile2 97:8f3abd5cf5ce 321 prev_err = 0;
kmsmile2 97:8f3abd5cf5ce 322 break;
kmsmile2 98:d8ead5e04047 323
kmsmile2 98:d8ead5e04047 324
kmsmile2 97:8f3abd5cf5ce 325 }
kmsmile2 97:8f3abd5cf5ce 326 }
kmsmile2 98:d8ead5e04047 327
kmsmile2 98:d8ead5e04047 328 }
kmsmile2 98:d8ead5e04047 329 //if(dist < 25)
kmsmile2 98:d8ead5e04047 330 // {
kmsmile2 98:d8ead5e04047 331 // int count=0;
kmsmile2 98:d8ead5e04047 332 // while(count<100){
kmsmile2 98:d8ead5e04047 333 // if(dist>10) break;
kmsmile2 98:d8ead5e04047 334 // count++;
kmsmile2 98:d8ead5e04047 335 // }
kmsmile2 98:d8ead5e04047 336 // if(count<100) continue;
kmsmile2 98:d8ead5e04047 337 // while(1)
kmsmile2 98:d8ead5e04047 338 // {
kmsmile2 98:d8ead5e04047 339 //
kmsmile2 98:d8ead5e04047 340 // motorDriver.user_forward(0,0.2);
kmsmile2 98:d8ead5e04047 341 //
kmsmile2 98:d8ead5e04047 342 // //pc.printf("distance: %d\r\n", dist);
kmsmile2 98:d8ead5e04047 343 // pc.printf("position: %d\r\n", position);
kmsmile2 98:d8ead5e04047 344 // position=trs.readLine(sensorValues,0);
kmsmile2 98:d8ead5e04047 345 // if(position > 2000)
kmsmile2 98:d8ead5e04047 346 // {
kmsmile2 98:d8ead5e04047 347 // int_err = 0;
kmsmile2 98:d8ead5e04047 348 // err = 0;
kmsmile2 98:d8ead5e04047 349 // prev_err = 0;
kmsmile2 98:d8ead5e04047 350 // break;
kmsmile2 98:d8ead5e04047 351 // }
kmsmile2 98:d8ead5e04047 352 // }
kmsmile2 98:d8ead5e04047 353 // }
kmsmile2 97:8f3abd5cf5ce 354
kmsmile2 97:8f3abd5cf5ce 355
kmsmile2 97:8f3abd5cf5ce 356 err=(int)position-2000; // error>0 --> right, error<0 --> left
kmsmile2 97:8f3abd5cf5ce 357 int_err+=err;
kmsmile2 97:8f3abd5cf5ce 358
kmsmile2 97:8f3abd5cf5ce 359 const float kp = pval;
kmsmile2 97:8f3abd5cf5ce 360 const float kd = dval;
kmsmile2 97:8f3abd5cf5ce 361 const float ki = ival;
kmsmile2 97:8f3abd5cf5ce 362
kmsmile2 97:8f3abd5cf5ce 363 float power_difference = kp*err + kd*(err-prev_err) + ki*int_err; //+derivative; //error를 누적하는 것
kmsmile2 97:8f3abd5cf5ce 364 prev_err = err;
kmsmile2 97:8f3abd5cf5ce 365
kmsmile2 97:8f3abd5cf5ce 366 const float maximum=vel;
kmsmile2 97:8f3abd5cf5ce 367 if(power_difference > maximum)
kmsmile2 97:8f3abd5cf5ce 368 power_difference = maximum;
kmsmile2 97:8f3abd5cf5ce 369 if(power_difference < -maximum)
kmsmile2 97:8f3abd5cf5ce 370 power_difference = -maximum;
kmsmile2 97:8f3abd5cf5ce 371
kmsmile2 97:8f3abd5cf5ce 372 if(power_difference<0)
kmsmile2 99:6efb1c0a6a68 373 motorDriver.user_forward((maximum)/255,(maximum+power_difference-10)/255);
kmsmile2 97:8f3abd5cf5ce 374 else
kmsmile2 99:6efb1c0a6a68 375 motorDriver.user_forward((maximum-power_difference)/255,(maximum-10)/255);
kmsmile2 97:8f3abd5cf5ce 376
kmsmile2 99:6efb1c0a6a68 377 // pc.printf("position value: %d\r\n", position);
kmsmile2 97:8f3abd5cf5ce 378
kmsmile2 97:8f3abd5cf5ce 379 //pc.printf("cnt: %d\r\n", cnt);
kmsmile2 99:6efb1c0a6a68 380 //for(int i=0;i<5;i++){
kmsmile2 99:6efb1c0a6a68 381 // pc.printf("%d\r\n",sensorValues[i]);
kmsmile2 99:6efb1c0a6a68 382 // }
kmsmile2 99:6efb1c0a6a68 383 if((sensorValues[0] > 650 ) && (sensorValues[1]>650 ) && (sensorValues[2]>650 ) && (sensorValues[3]>650 && sensorValues[3] < 850) && (sensorValues[4]>650&& sensorValues[4] < 850) )
kmsmile2 97:8f3abd5cf5ce 384 { // 5 IR sensor are on black
kmsmile2 97:8f3abd5cf5ce 385 timer.stop();
kmsmile2 97:8f3abd5cf5ce 386 t=timer.read();
kmsmile2 97:8f3abd5cf5ce 387 update_display();
kmsmile2 98:d8ead5e04047 388 motorDriver.stop();
kmsmile2 98:d8ead5e04047 389 for(int z=48;z>=0;z=z-4){
kmsmile2 98:d8ead5e04047 390 ws.write_offsets(px.getBuf(),z,z,z);
kmsmile2 98:d8ead5e04047 391 wait(0.1);
kmsmile2 98:d8ead5e04047 392 }
kmsmile2 98:d8ead5e04047 393
kmsmile2 97:8f3abd5cf5ce 394 button = 0x1C;
kmsmile2 97:8f3abd5cf5ce 395 break;
kmsmile2 97:8f3abd5cf5ce 396 }
kmsmile2 97:8f3abd5cf5ce 397 }
kmsmile2 97:8f3abd5cf5ce 398 break;
kmsmile2 97:8f3abd5cf5ce 399
kmsmile2 97:8f3abd5cf5ce 400 case 0x42:
kmsmile2 97:8f3abd5cf5ce 401 // 7 button (read sensor values)
kmsmile2 98:d8ead5e04047 402 position=trs.readLine(sensorValues,0);
kmsmile2 98:d8ead5e04047 403 for(int i=0; i<5; i++) {
kmsmile2 98:d8ead5e04047 404 pc.printf("%d\r\n",sensorValues[i]);
kmsmile2 97:8f3abd5cf5ce 405 }
kmsmile2 98:d8ead5e04047 406 pc.printf("done!\r\n");
kmsmile2 98:d8ead5e04047 407 button = 0x1C;
kmsmile2 98:d8ead5e04047 408 break;
kmsmile2 98:d8ead5e04047 409
kmsmile2 97:8f3abd5cf5ce 410
kmsmile2 97:8f3abd5cf5ce 411 case 0x4A:
kmsmile2 97:8f3abd5cf5ce 412 // 9 button (read position)
kmsmile2 97:8f3abd5cf5ce 413 int j = 0;
kmsmile2 97:8f3abd5cf5ce 414 while(j!=100)
kmsmile2 97:8f3abd5cf5ce 415 {
kmsmile2 97:8f3abd5cf5ce 416 j++;
kmsmile2 97:8f3abd5cf5ce 417 position=trs.readLine(sensorValues,0);
kmsmile2 97:8f3abd5cf5ce 418 pc.printf("%d\r\n",position);
kmsmile2 97:8f3abd5cf5ce 419 }
kmsmile2 97:8f3abd5cf5ce 420 button = 0x1C;
kmsmile2 97:8f3abd5cf5ce 421 break;
kmsmile2 98:d8ead5e04047 422 default:
kmsmile2 97:8f3abd5cf5ce 423 // wrong button
kmsmile2 97:8f3abd5cf5ce 424 pc.printf("wrong button!\r\n");
kmsmile2 97:8f3abd5cf5ce 425 break;
kmsmile2 97:8f3abd5cf5ce 426 }
kmsmile2 97:8f3abd5cf5ce 427 }
kmsmile2 97:8f3abd5cf5ce 428 }