course1
Dependencies: WS2812 PixelArray Adafruit_GFX HC_SR04_Ultrasonic_Library
main.cpp@98:d8ead5e04047, 2019-06-13 (annotated)
- Committer:
- kmsmile2
- Date:
- Thu Jun 13 11:12:37 2019 +0000
- Revision:
- 98:d8ead5e04047
- Parent:
- 97:8f3abd5cf5ce
- Child:
- 99:43e306bf6f13
- Child:
- 100:6efb1c0a6a68
Minsu
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 | 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 | 97:8f3abd5cf5ce | 249 | // 100+ button (Calibrate) |
kmsmile2 | 97:8f3abd5cf5ce | 250 | pc.printf("calibrate!\r\n"); |
kmsmile2 | 97:8f3abd5cf5ce | 251 | trs.calibrate(); |
kmsmile2 | 97:8f3abd5cf5ce | 252 | pc.printf("calibrate done\r\n"); |
kmsmile2 | 97:8f3abd5cf5ce | 253 | wait(0.2); |
kmsmile2 | 97:8f3abd5cf5ce | 254 | button = 0x1C; |
kmsmile2 | 97:8f3abd5cf5ce | 255 | break; |
kmsmile2 | 97:8f3abd5cf5ce | 256 | |
kmsmile2 | 97:8f3abd5cf5ce | 257 | case 0x18: |
kmsmile2 | 97:8f3abd5cf5ce | 258 | // 2 button (move forward) |
kmsmile2 | 97:8f3abd5cf5ce | 259 | motorDriver.user_forward(0.3,0.3); |
kmsmile2 | 98:d8ead5e04047 | 260 | trs.calibrate(); |
kmsmile2 | 97:8f3abd5cf5ce | 261 | wait(0.1); |
kmsmile2 | 97:8f3abd5cf5ce | 262 | button = 0x1C; |
kmsmile2 | 97:8f3abd5cf5ce | 263 | break; |
kmsmile2 | 97:8f3abd5cf5ce | 264 | |
kmsmile2 | 97:8f3abd5cf5ce | 265 | case 0x52: |
kmsmile2 | 97:8f3abd5cf5ce | 266 | // 8 button (move backward) |
kmsmile2 | 97:8f3abd5cf5ce | 267 | motorDriver.user_backward(0.3,0.3); |
kmsmile2 | 98:d8ead5e04047 | 268 | trs.calibrate(); |
kmsmile2 | 97:8f3abd5cf5ce | 269 | wait(0.1); |
kmsmile2 | 97:8f3abd5cf5ce | 270 | button = 0x1C; |
kmsmile2 | 97:8f3abd5cf5ce | 271 | break; |
kmsmile2 | 97:8f3abd5cf5ce | 272 | |
kmsmile2 | 97:8f3abd5cf5ce | 273 | case 0x5A: |
kmsmile2 | 97:8f3abd5cf5ce | 274 | // 6 button (clockwise turn) |
kmsmile2 | 97:8f3abd5cf5ce | 275 | motorDriver.user_right(0.3,0.1); |
kmsmile2 | 97:8f3abd5cf5ce | 276 | wait(0.1); |
kmsmile2 | 97:8f3abd5cf5ce | 277 | button = 0x1C; |
kmsmile2 | 97:8f3abd5cf5ce | 278 | break; |
kmsmile2 | 97:8f3abd5cf5ce | 279 | |
kmsmile2 | 97:8f3abd5cf5ce | 280 | case 0x08: |
kmsmile2 | 97:8f3abd5cf5ce | 281 | // 4 button (counter clockwise turn) |
kmsmile2 | 97:8f3abd5cf5ce | 282 | motorDriver.user_left(0.1,0.3); |
kmsmile2 | 97:8f3abd5cf5ce | 283 | wait(0.1); |
kmsmile2 | 97:8f3abd5cf5ce | 284 | button = 0x1C; |
kmsmile2 | 97:8f3abd5cf5ce | 285 | break; |
kmsmile2 | 97:8f3abd5cf5ce | 286 | |
kmsmile2 | 97:8f3abd5cf5ce | 287 | case 0x1C: |
kmsmile2 | 97:8f3abd5cf5ce | 288 | // 5 button (motor stop) |
kmsmile2 | 97:8f3abd5cf5ce | 289 | motorDriver.stop(); |
kmsmile2 | 97:8f3abd5cf5ce | 290 | break; |
kmsmile2 | 97:8f3abd5cf5ce | 291 | |
kmsmile2 | 97:8f3abd5cf5ce | 292 | case 0x0D: |
kmsmile2 | 97:8f3abd5cf5ce | 293 | // 200+ button (line tracer) |
kmsmile2 | 97:8f3abd5cf5ce | 294 | t=0; |
kmsmile2 | 97:8f3abd5cf5ce | 295 | timer.reset(); |
kmsmile2 | 97:8f3abd5cf5ce | 296 | timer.start(); |
kmsmile2 | 98:d8ead5e04047 | 297 | ultra.setMode(false); // have updates every .1 seconds and try only once. |
kmsmile2 | 98:d8ead5e04047 | 298 | |
kmsmile2 | 97:8f3abd5cf5ce | 299 | while(1) |
kmsmile2 | 97:8f3abd5cf5ce | 300 | { |
kmsmile2 | 97:8f3abd5cf5ce | 301 | t=timer.read(); |
kmsmile2 | 97:8f3abd5cf5ce | 302 | position=trs.readLine(sensorValues,0); |
kmsmile2 | 98:d8ead5e04047 | 303 | ultra.trig(); |
kmsmile2 | 98:d8ead5e04047 | 304 | dist = ultra.getDistance(); |
kmsmile2 | 98:d8ead5e04047 | 305 | pc.printf("dist:%d\r\n", dist); |
kmsmile2 | 98:d8ead5e04047 | 306 | if(dist<=21){ |
kmsmile2 | 98:d8ead5e04047 | 307 | motorDriver.user_left(0.2,0.2); |
kmsmile2 | 98:d8ead5e04047 | 308 | wait(0.1); |
kmsmile2 | 98:d8ead5e04047 | 309 | while(1){ |
kmsmile2 | 98:d8ead5e04047 | 310 | |
kmsmile2 | 98:d8ead5e04047 | 311 | // pc.printf("distance: %d\r\n", dist); |
kmsmile2 | 98:d8ead5e04047 | 312 | // pc.printf("position: %d\r\n", position); |
kmsmile2 | 97:8f3abd5cf5ce | 313 | position=trs.readLine(sensorValues,0); |
kmsmile2 | 97:8f3abd5cf5ce | 314 | if(position > 2000) |
kmsmile2 | 97:8f3abd5cf5ce | 315 | { |
kmsmile2 | 97:8f3abd5cf5ce | 316 | int_err = 0; |
kmsmile2 | 97:8f3abd5cf5ce | 317 | err = 0; |
kmsmile2 | 97:8f3abd5cf5ce | 318 | prev_err = 0; |
kmsmile2 | 97:8f3abd5cf5ce | 319 | break; |
kmsmile2 | 98:d8ead5e04047 | 320 | |
kmsmile2 | 98:d8ead5e04047 | 321 | |
kmsmile2 | 97:8f3abd5cf5ce | 322 | } |
kmsmile2 | 97:8f3abd5cf5ce | 323 | } |
kmsmile2 | 98:d8ead5e04047 | 324 | |
kmsmile2 | 98:d8ead5e04047 | 325 | } |
kmsmile2 | 98:d8ead5e04047 | 326 | //if(dist < 25) |
kmsmile2 | 98:d8ead5e04047 | 327 | // { |
kmsmile2 | 98:d8ead5e04047 | 328 | // int count=0; |
kmsmile2 | 98:d8ead5e04047 | 329 | // while(count<100){ |
kmsmile2 | 98:d8ead5e04047 | 330 | // if(dist>10) break; |
kmsmile2 | 98:d8ead5e04047 | 331 | // count++; |
kmsmile2 | 98:d8ead5e04047 | 332 | // } |
kmsmile2 | 98:d8ead5e04047 | 333 | // if(count<100) continue; |
kmsmile2 | 98:d8ead5e04047 | 334 | // while(1) |
kmsmile2 | 98:d8ead5e04047 | 335 | // { |
kmsmile2 | 98:d8ead5e04047 | 336 | // |
kmsmile2 | 98:d8ead5e04047 | 337 | // motorDriver.user_forward(0,0.2); |
kmsmile2 | 98:d8ead5e04047 | 338 | // |
kmsmile2 | 98:d8ead5e04047 | 339 | // //pc.printf("distance: %d\r\n", dist); |
kmsmile2 | 98:d8ead5e04047 | 340 | // pc.printf("position: %d\r\n", position); |
kmsmile2 | 98:d8ead5e04047 | 341 | // position=trs.readLine(sensorValues,0); |
kmsmile2 | 98:d8ead5e04047 | 342 | // if(position > 2000) |
kmsmile2 | 98:d8ead5e04047 | 343 | // { |
kmsmile2 | 98:d8ead5e04047 | 344 | // int_err = 0; |
kmsmile2 | 98:d8ead5e04047 | 345 | // err = 0; |
kmsmile2 | 98:d8ead5e04047 | 346 | // prev_err = 0; |
kmsmile2 | 98:d8ead5e04047 | 347 | // break; |
kmsmile2 | 98:d8ead5e04047 | 348 | // } |
kmsmile2 | 98:d8ead5e04047 | 349 | // } |
kmsmile2 | 98:d8ead5e04047 | 350 | // } |
kmsmile2 | 97:8f3abd5cf5ce | 351 | |
kmsmile2 | 97:8f3abd5cf5ce | 352 | |
kmsmile2 | 97:8f3abd5cf5ce | 353 | err=(int)position-2000; // error>0 --> right, error<0 --> left |
kmsmile2 | 97:8f3abd5cf5ce | 354 | int_err+=err; |
kmsmile2 | 97:8f3abd5cf5ce | 355 | |
kmsmile2 | 97:8f3abd5cf5ce | 356 | const float kp = pval; |
kmsmile2 | 97:8f3abd5cf5ce | 357 | const float kd = dval; |
kmsmile2 | 97:8f3abd5cf5ce | 358 | const float ki = ival; |
kmsmile2 | 97:8f3abd5cf5ce | 359 | |
kmsmile2 | 97:8f3abd5cf5ce | 360 | float power_difference = kp*err + kd*(err-prev_err) + ki*int_err; //+derivative; //error를 누적하는 것 |
kmsmile2 | 97:8f3abd5cf5ce | 361 | prev_err = err; |
kmsmile2 | 97:8f3abd5cf5ce | 362 | |
kmsmile2 | 97:8f3abd5cf5ce | 363 | const float maximum=vel; |
kmsmile2 | 97:8f3abd5cf5ce | 364 | if(power_difference > maximum) |
kmsmile2 | 97:8f3abd5cf5ce | 365 | power_difference = maximum; |
kmsmile2 | 97:8f3abd5cf5ce | 366 | if(power_difference < -maximum) |
kmsmile2 | 97:8f3abd5cf5ce | 367 | power_difference = -maximum; |
kmsmile2 | 97:8f3abd5cf5ce | 368 | |
kmsmile2 | 97:8f3abd5cf5ce | 369 | if(power_difference<0) |
kmsmile2 | 98:d8ead5e04047 | 370 | motorDriver.user_forward((maximum-10)/255,(maximum+power_difference)/255); |
kmsmile2 | 97:8f3abd5cf5ce | 371 | else |
kmsmile2 | 98:d8ead5e04047 | 372 | motorDriver.user_forward((maximum-power_difference)/255,(maximum+10)/255); |
kmsmile2 | 97:8f3abd5cf5ce | 373 | |
kmsmile2 | 97:8f3abd5cf5ce | 374 | pc.printf("position value: %d\r\n", position); |
kmsmile2 | 97:8f3abd5cf5ce | 375 | |
kmsmile2 | 97:8f3abd5cf5ce | 376 | //pc.printf("cnt: %d\r\n", cnt); |
kmsmile2 | 97:8f3abd5cf5ce | 377 | for(int i=0;i<5;i++){ |
kmsmile2 | 97:8f3abd5cf5ce | 378 | pc.printf("%d\r\n",sensorValues[i]); |
kmsmile2 | 97:8f3abd5cf5ce | 379 | } |
kmsmile2 | 97:8f3abd5cf5ce | 380 | if((sensorValues[0] > 650) && (sensorValues[1]>650) && (sensorValues[2]>650) && (sensorValues[3]>650) && (sensorValues[4]>650)) |
kmsmile2 | 97:8f3abd5cf5ce | 381 | { // 5 IR sensor are on black |
kmsmile2 | 97:8f3abd5cf5ce | 382 | timer.stop(); |
kmsmile2 | 97:8f3abd5cf5ce | 383 | t=timer.read(); |
kmsmile2 | 97:8f3abd5cf5ce | 384 | update_display(); |
kmsmile2 | 98:d8ead5e04047 | 385 | motorDriver.stop(); |
kmsmile2 | 98:d8ead5e04047 | 386 | for(int z=48;z>=0;z=z-4){ |
kmsmile2 | 98:d8ead5e04047 | 387 | ws.write_offsets(px.getBuf(),z,z,z); |
kmsmile2 | 98:d8ead5e04047 | 388 | wait(0.1); |
kmsmile2 | 98:d8ead5e04047 | 389 | } |
kmsmile2 | 98:d8ead5e04047 | 390 | |
kmsmile2 | 97:8f3abd5cf5ce | 391 | button = 0x1C; |
kmsmile2 | 97:8f3abd5cf5ce | 392 | break; |
kmsmile2 | 97:8f3abd5cf5ce | 393 | } |
kmsmile2 | 97:8f3abd5cf5ce | 394 | } |
kmsmile2 | 97:8f3abd5cf5ce | 395 | break; |
kmsmile2 | 97:8f3abd5cf5ce | 396 | |
kmsmile2 | 97:8f3abd5cf5ce | 397 | case 0x42: |
kmsmile2 | 97:8f3abd5cf5ce | 398 | // 7 button (read sensor values) |
kmsmile2 | 98:d8ead5e04047 | 399 | position=trs.readLine(sensorValues,0); |
kmsmile2 | 98:d8ead5e04047 | 400 | for(int i=0; i<5; i++) { |
kmsmile2 | 98:d8ead5e04047 | 401 | pc.printf("%d\r\n",sensorValues[i]); |
kmsmile2 | 97:8f3abd5cf5ce | 402 | } |
kmsmile2 | 98:d8ead5e04047 | 403 | pc.printf("done!\r\n"); |
kmsmile2 | 98:d8ead5e04047 | 404 | button = 0x1C; |
kmsmile2 | 98:d8ead5e04047 | 405 | break; |
kmsmile2 | 98:d8ead5e04047 | 406 | |
kmsmile2 | 97:8f3abd5cf5ce | 407 | |
kmsmile2 | 97:8f3abd5cf5ce | 408 | case 0x4A: |
kmsmile2 | 97:8f3abd5cf5ce | 409 | // 9 button (read position) |
kmsmile2 | 97:8f3abd5cf5ce | 410 | int j = 0; |
kmsmile2 | 97:8f3abd5cf5ce | 411 | while(j!=100) |
kmsmile2 | 97:8f3abd5cf5ce | 412 | { |
kmsmile2 | 97:8f3abd5cf5ce | 413 | j++; |
kmsmile2 | 97:8f3abd5cf5ce | 414 | position=trs.readLine(sensorValues,0); |
kmsmile2 | 97:8f3abd5cf5ce | 415 | pc.printf("%d\r\n",position); |
kmsmile2 | 97:8f3abd5cf5ce | 416 | } |
kmsmile2 | 97:8f3abd5cf5ce | 417 | button = 0x1C; |
kmsmile2 | 97:8f3abd5cf5ce | 418 | break; |
kmsmile2 | 98:d8ead5e04047 | 419 | default: |
kmsmile2 | 97:8f3abd5cf5ce | 420 | // wrong button |
kmsmile2 | 97:8f3abd5cf5ce | 421 | pc.printf("wrong button!\r\n"); |
kmsmile2 | 97:8f3abd5cf5ce | 422 | break; |
kmsmile2 | 97:8f3abd5cf5ce | 423 | } |
kmsmile2 | 97:8f3abd5cf5ce | 424 | } |
kmsmile2 | 97:8f3abd5cf5ce | 425 | } |