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@97:8f3abd5cf5ce, 2019-06-12 (annotated)

Committer:
kmsmile2
Date:
Wed Jun 12 08:20:53 2019 +0000
Revision:
97:8f3abd5cf5ce
Parent:
88:bea4f2daa48c
Child:
98:d8ead5e04047
Eron_alphabot

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