course1
Dependencies: WS2812 PixelArray Adafruit_GFX HC_SR04_Ultrasonic_Library
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?
User | Revision | Line number | New 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 | } |