dd
Dependencies: Final HCSR04 TB6612FNG
main.cpp@97:b483e656bd14, 2019-06-16 (annotated)
- Committer:
- eunmango
- Date:
- Sun Jun 16 04:44:35 2019 +0000
- Revision:
- 97:b483e656bd14
- Parent:
- 88:bea4f2daa48c
t
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
eunmango | 97:b483e656bd14 | 1 | #include "mbed.h" |
eunmango | 97:b483e656bd14 | 2 | #include <iostream> |
eunmango | 97:b483e656bd14 | 3 | #include "HCSR04.h" |
eunmango | 97:b483e656bd14 | 4 | #include <list> |
eunmango | 97:b483e656bd14 | 5 | #include <string.h> |
eunmango | 97:b483e656bd14 | 6 | #include <IRremote.h> |
eunmango | 97:b483e656bd14 | 7 | #include "TB6612FNG.h" |
eunmango | 97:b483e656bd14 | 8 | //#define PCF8574_ADDR (0x40) |
eunmango | 97:b483e656bd14 | 9 | #define LEFT 127 |
eunmango | 97:b483e656bd14 | 10 | #define RIGHT 191 |
eunmango | 97:b483e656bd14 | 11 | #define NUMSENSORS 5 |
eunmango | 97:b483e656bd14 | 12 | #define MAX 1023 |
eunmango | 97:b483e656bd14 | 13 | #define MIN 0 |
eunmango | 97:b483e656bd14 | 14 | #define TRU 1 |
eunmango | 97:b483e656bd14 | 15 | #define FALS 0 |
eunmango | 97:b483e656bd14 | 16 | #define MaxSpeed 255// max speed of the robot |
eunmango | 97:b483e656bd14 | 17 | #define BaseSpeed 155 |
eunmango | 97:b483e656bd14 | 18 | #define speedt 180 |
eunmango | 97:b483e656bd14 | 19 | |
eunmango | 97:b483e656bd14 | 20 | #include "SSD1306-Library.h" |
eunmango | 97:b483e656bd14 | 21 | #include "FreeSans9pt7b.h" |
eunmango | 97:b483e656bd14 | 22 | #include <time.h> |
eunmango | 97:b483e656bd14 | 23 | #include <math.h> |
eunmango | 97:b483e656bd14 | 24 | |
eunmango | 97:b483e656bd14 | 25 | #define KEY2 0xFF18E7 //Key:2 |
eunmango | 97:b483e656bd14 | 26 | #define KEY8 0xFF52AD //Key:8 |
eunmango | 97:b483e656bd14 | 27 | #define KEY4 0xFF08F7 //Key:4 |
eunmango | 97:b483e656bd14 | 28 | #define KEY6 0xFF5AA5 //Key:6 |
eunmango | 97:b483e656bd14 | 29 | #define KEY1 0xFF0CF3 //Key:1 |
eunmango | 97:b483e656bd14 | 30 | #define KEY3 0xFF5EA1 //Key:3 |
eunmango | 97:b483e656bd14 | 31 | #define KEY5 0xFF1CE3 //Key:5 |
eunmango | 97:b483e656bd14 | 32 | |
eunmango | 97:b483e656bd14 | 33 | |
eunmango | 97:b483e656bd14 | 34 | //I2C i2c(I2C_SDA, I2C_SCL); //D15,D14 |
eunmango | 97:b483e656bd14 | 35 | SPI spi(D11, D12, D13); |
eunmango | 97:b483e656bd14 | 36 | DigitalOut cs(D10,1); |
eunmango | 97:b483e656bd14 | 37 | Serial pc(PA_2, PA_3, 115200); //D1 D0 |
eunmango | 97:b483e656bd14 | 38 | IRrecv irrecv(D4); |
eunmango | 97:b483e656bd14 | 39 | |
eunmango | 97:b483e656bd14 | 40 | |
eunmango | 97:b483e656bd14 | 41 | int mode=1; |
eunmango | 97:b483e656bd14 | 42 | int index=0; |
eunmango | 97:b483e656bd14 | 43 | int rx_buffer[10]; |
eunmango | 97:b483e656bd14 | 44 | char arr[16]; |
eunmango | 97:b483e656bd14 | 45 | volatile int flag = 0; |
eunmango | 97:b483e656bd14 | 46 | float interval=0.1; //100ms |
eunmango | 97:b483e656bd14 | 47 | decode_results result; |
eunmango | 97:b483e656bd14 | 48 | |
eunmango | 97:b483e656bd14 | 49 | |
eunmango | 97:b483e656bd14 | 50 | |
eunmango | 97:b483e656bd14 | 51 | /////////sensors//////////// |
eunmango | 97:b483e656bd14 | 52 | |
eunmango | 97:b483e656bd14 | 53 | int temp=0; |
eunmango | 97:b483e656bd14 | 54 | int calibratedMin[NUMSENSORS]; |
eunmango | 97:b483e656bd14 | 55 | int calibratedMax[NUMSENSORS]; |
eunmango | 97:b483e656bd14 | 56 | unsigned int sensor_values[NUMSENSORS]; |
eunmango | 97:b483e656bd14 | 57 | int in_line[NUMSENSORS]; |
eunmango | 97:b483e656bd14 | 58 | int ultra_flag=0; |
eunmango | 97:b483e656bd14 | 59 | float position; |
eunmango | 97:b483e656bd14 | 60 | |
eunmango | 97:b483e656bd14 | 61 | /////////sensors//////////// |
eunmango | 97:b483e656bd14 | 62 | |
eunmango | 97:b483e656bd14 | 63 | /////////Ultrasonic//////////// |
eunmango | 97:b483e656bd14 | 64 | Ultrasonic ultra(D3, D2); |
eunmango | 97:b483e656bd14 | 65 | int ultra_result=10; |
eunmango | 97:b483e656bd14 | 66 | /////////Ultrasonic//////////// |
eunmango | 97:b483e656bd14 | 67 | |
eunmango | 97:b483e656bd14 | 68 | |
eunmango | 97:b483e656bd14 | 69 | //////////motor//////////// |
eunmango | 97:b483e656bd14 | 70 | PwmOut motor_pwmA(PB_10); //D6 |
eunmango | 97:b483e656bd14 | 71 | PwmOut motor_pwmb(PB_4); // D5 |
eunmango | 97:b483e656bd14 | 72 | AnalogIn rightMotor2(A0); |
eunmango | 97:b483e656bd14 | 73 | AnalogIn rightMotor1(A1); |
eunmango | 97:b483e656bd14 | 74 | AnalogIn leftMotor2(A3); |
eunmango | 97:b483e656bd14 | 75 | AnalogIn leftMotor1(A2); |
eunmango | 97:b483e656bd14 | 76 | |
eunmango | 97:b483e656bd14 | 77 | TB6612FNG leftmotor(PB_10, A1, A0); |
eunmango | 97:b483e656bd14 | 78 | TB6612FNG rightmotor(PB_4, A2, A3); |
eunmango | 97:b483e656bd14 | 79 | float last_proportional; |
eunmango | 97:b483e656bd14 | 80 | float integral; |
eunmango | 97:b483e656bd14 | 81 | float kp = 0.5; |
eunmango | 97:b483e656bd14 | 82 | float kd = 0.1; |
eunmango | 97:b483e656bd14 | 83 | int lastError; |
eunmango | 97:b483e656bd14 | 84 | //////////motor//////////// |
eunmango | 97:b483e656bd14 | 85 | |
eunmango | 97:b483e656bd14 | 86 | void IRForward(void) |
eunmango | 97:b483e656bd14 | 87 | { |
eunmango | 97:b483e656bd14 | 88 | leftmotor.setSpeed(0.1); |
eunmango | 97:b483e656bd14 | 89 | rightmotor.setSpeed(0.1); |
eunmango | 97:b483e656bd14 | 90 | |
eunmango | 97:b483e656bd14 | 91 | } |
eunmango | 97:b483e656bd14 | 92 | |
eunmango | 97:b483e656bd14 | 93 | void ultraLeft(){ |
eunmango | 97:b483e656bd14 | 94 | leftmotor.setSpeed(-0.13); |
eunmango | 97:b483e656bd14 | 95 | rightmotor.setSpeed(0.15); |
eunmango | 97:b483e656bd14 | 96 | } |
eunmango | 97:b483e656bd14 | 97 | |
eunmango | 97:b483e656bd14 | 98 | void left(void) |
eunmango | 97:b483e656bd14 | 99 | { |
eunmango | 97:b483e656bd14 | 100 | leftmotor.setSpeed(0.03); |
eunmango | 97:b483e656bd14 | 101 | rightmotor.setSpeed(0.15); |
eunmango | 97:b483e656bd14 | 102 | } |
eunmango | 97:b483e656bd14 | 103 | |
eunmango | 97:b483e656bd14 | 104 | |
eunmango | 97:b483e656bd14 | 105 | void right(void) |
eunmango | 97:b483e656bd14 | 106 | { |
eunmango | 97:b483e656bd14 | 107 | leftmotor.setSpeed(0.15); |
eunmango | 97:b483e656bd14 | 108 | rightmotor.setSpeed(0.03); |
eunmango | 97:b483e656bd14 | 109 | } |
eunmango | 97:b483e656bd14 | 110 | |
eunmango | 97:b483e656bd14 | 111 | void stop(void) |
eunmango | 97:b483e656bd14 | 112 | { |
eunmango | 97:b483e656bd14 | 113 | leftmotor.setSpeed(0); |
eunmango | 97:b483e656bd14 | 114 | rightmotor.setSpeed(0); |
eunmango | 97:b483e656bd14 | 115 | } |
eunmango | 97:b483e656bd14 | 116 | |
eunmango | 97:b483e656bd14 | 117 | void CalibrationMotor(void) |
eunmango | 97:b483e656bd14 | 118 | { |
eunmango | 97:b483e656bd14 | 119 | leftmotor.setSpeed(0.15); |
eunmango | 97:b483e656bd14 | 120 | rightmotor.setSpeed(-0.15); |
eunmango | 97:b483e656bd14 | 121 | } |
eunmango | 97:b483e656bd14 | 122 | |
eunmango | 97:b483e656bd14 | 123 | void backward(void) |
eunmango | 97:b483e656bd14 | 124 | { |
eunmango | 97:b483e656bd14 | 125 | leftmotor.setSpeed(-0.1); |
eunmango | 97:b483e656bd14 | 126 | rightmotor.setSpeed(-0.1); |
eunmango | 97:b483e656bd14 | 127 | } |
eunmango | 97:b483e656bd14 | 128 | |
mbed_official | 82:abf1b1785bd7 | 129 | |
eunmango | 97:b483e656bd14 | 130 | //bool pcf8574_write(uint8_t data){ |
eunmango | 97:b483e656bd14 | 131 | // return i2c.write(PCF8574_ADDR, (char*) &data, 1, 0) == 0; |
eunmango | 97:b483e656bd14 | 132 | //} |
eunmango | 97:b483e656bd14 | 133 | // |
eunmango | 97:b483e656bd14 | 134 | //bool pcf8574_read(uint8_t* data){ |
eunmango | 97:b483e656bd14 | 135 | // return i2c.read(PCF8574_ADDR, (char*) data, 1, 0) == 0; |
eunmango | 97:b483e656bd14 | 136 | //} |
eunmango | 97:b483e656bd14 | 137 | // |
eunmango | 97:b483e656bd14 | 138 | //int pcf8574_test(uint8_t value){ |
eunmango | 97:b483e656bd14 | 139 | // |
eunmango | 97:b483e656bd14 | 140 | // int ret; |
eunmango | 97:b483e656bd14 | 141 | // uint8_t data=0; |
eunmango | 97:b483e656bd14 | 142 | // |
eunmango | 97:b483e656bd14 | 143 | // ret = pcf8574_write(value); |
eunmango | 97:b483e656bd14 | 144 | // if(!ret) return -1; |
eunmango | 97:b483e656bd14 | 145 | // |
eunmango | 97:b483e656bd14 | 146 | // ret = pcf8574_read(&data); |
eunmango | 97:b483e656bd14 | 147 | // if(!ret) return -2; |
eunmango | 97:b483e656bd14 | 148 | // |
eunmango | 97:b483e656bd14 | 149 | // return data; |
eunmango | 97:b483e656bd14 | 150 | //} |
eunmango | 97:b483e656bd14 | 151 | // |
eunmango | 97:b483e656bd14 | 152 | //void detected(void) |
eunmango | 97:b483e656bd14 | 153 | //{ |
eunmango | 97:b483e656bd14 | 154 | // pcf8574_test(0xff); |
eunmango | 97:b483e656bd14 | 155 | // while(pcf8574_test(0xff)<255) pcf8574_test(0x00); |
eunmango | 97:b483e656bd14 | 156 | //} |
eunmango | 97:b483e656bd14 | 157 | |
eunmango | 97:b483e656bd14 | 158 | int state=0; //0:forward / 1:left / 2:right |
eunmango | 97:b483e656bd14 | 159 | int cnt=0; |
eunmango | 97:b483e656bd14 | 160 | int period_us; |
eunmango | 97:b483e656bd14 | 161 | int beat_ms; |
Jonathan Austin |
0:2757d7abb7d9 | 162 | |
eunmango | 97:b483e656bd14 | 163 | void TRSensors(void) { |
eunmango | 97:b483e656bd14 | 164 | |
eunmango | 97:b483e656bd14 | 165 | for(int i=0; i< NUMSENSORS ; i++) { |
eunmango | 97:b483e656bd14 | 166 | calibratedMin[i]= MAX; |
eunmango | 97:b483e656bd14 | 167 | calibratedMax[i] = MIN; |
eunmango | 97:b483e656bd14 | 168 | } |
eunmango | 97:b483e656bd14 | 169 | |
eunmango | 97:b483e656bd14 | 170 | } |
eunmango | 97:b483e656bd14 | 171 | |
eunmango | 97:b483e656bd14 | 172 | void set_TLC1543 (void) { |
eunmango | 97:b483e656bd14 | 173 | |
eunmango | 97:b483e656bd14 | 174 | int value; |
eunmango | 97:b483e656bd14 | 175 | for(int i=0; i<6 ; i++){ |
eunmango | 97:b483e656bd14 | 176 | cs=0; |
eunmango | 97:b483e656bd14 | 177 | wait_us(2); |
eunmango | 97:b483e656bd14 | 178 | value=spi.write(i<<12); |
eunmango | 97:b483e656bd14 | 179 | cs=1; |
eunmango | 97:b483e656bd14 | 180 | wait_us(21); |
eunmango | 97:b483e656bd14 | 181 | cs=0; |
eunmango | 97:b483e656bd14 | 182 | wait_us(2); |
eunmango | 97:b483e656bd14 | 183 | value=spi.write(i<<12); |
eunmango | 97:b483e656bd14 | 184 | //pc.printf("%d th : 0x%X\r\n", i, value); |
eunmango | 97:b483e656bd14 | 185 | cs=1; |
eunmango | 97:b483e656bd14 | 186 | wait_us(21); |
eunmango | 97:b483e656bd14 | 187 | sensor_values[i]=value; |
eunmango | 97:b483e656bd14 | 188 | } |
eunmango | 97:b483e656bd14 | 189 | } |
eunmango | 97:b483e656bd14 | 190 | |
eunmango | 97:b483e656bd14 | 191 | void calibrate(void){ |
eunmango | 97:b483e656bd14 | 192 | int i; |
eunmango | 97:b483e656bd14 | 193 | |
eunmango | 97:b483e656bd14 | 194 | unsigned int max_sensor_values[NUMSENSORS]; |
eunmango | 97:b483e656bd14 | 195 | unsigned int min_sensor_values[NUMSENSORS]; |
eunmango | 97:b483e656bd14 | 196 | |
eunmango | 97:b483e656bd14 | 197 | int j; |
eunmango | 97:b483e656bd14 | 198 | for(j=0; j <10 ; j++){ |
eunmango | 97:b483e656bd14 | 199 | set_TLC1543(); |
eunmango | 97:b483e656bd14 | 200 | for(i=0; i<NUMSENSORS; i++){ |
eunmango | 97:b483e656bd14 | 201 | if(j==0 || max_sensor_values[i] < sensor_values[i]) max_sensor_values[i] = sensor_values[i]; |
eunmango | 97:b483e656bd14 | 202 | if(j==0 || min_sensor_values[i] > sensor_values[i]) min_sensor_values[i] = sensor_values[i]; |
eunmango | 97:b483e656bd14 | 203 | } |
eunmango | 97:b483e656bd14 | 204 | } |
eunmango | 97:b483e656bd14 | 205 | |
eunmango | 97:b483e656bd14 | 206 | for(i=0; i<NUMSENSORS; i++){ |
eunmango | 97:b483e656bd14 | 207 | if(min_sensor_values[i] > calibratedMax[i]) calibratedMax[i] = min_sensor_values[i]; |
eunmango | 97:b483e656bd14 | 208 | if(max_sensor_values[i] < calibratedMin[i]) calibratedMin[i] = max_sensor_values[i]; |
eunmango | 97:b483e656bd14 | 209 | } |
eunmango | 97:b483e656bd14 | 210 | |
eunmango | 97:b483e656bd14 | 211 | } |
Jonathan Austin |
0:2757d7abb7d9 | 212 | |
eunmango | 97:b483e656bd14 | 213 | void readCalibrated(){ |
eunmango | 97:b483e656bd14 | 214 | int i; |
eunmango | 97:b483e656bd14 | 215 | set_TLC1543(); |
eunmango | 97:b483e656bd14 | 216 | for(i=0; i<NUMSENSORS ; i++){ |
eunmango | 97:b483e656bd14 | 217 | //unsigned int calmin, calmax; |
eunmango | 97:b483e656bd14 | 218 | unsigned int denominator; |
eunmango | 97:b483e656bd14 | 219 | denominator = calibratedMax[i] - calibratedMin[i]; |
eunmango | 97:b483e656bd14 | 220 | signed int x =0; |
eunmango | 97:b483e656bd14 | 221 | if(denominator !=0) { |
eunmango | 97:b483e656bd14 | 222 | x = ( ( (signed long)sensor_values[i] ) -calibratedMin[i]) * 1000 / denominator; |
eunmango | 97:b483e656bd14 | 223 | } |
eunmango | 97:b483e656bd14 | 224 | if(x<0) x=0; |
eunmango | 97:b483e656bd14 | 225 | else if(x>1000) x= 1000; |
eunmango | 97:b483e656bd14 | 226 | else sensor_values[i] = x; |
eunmango | 97:b483e656bd14 | 227 | } |
eunmango | 97:b483e656bd14 | 228 | } |
mbed_official | 88:bea4f2daa48c | 229 | |
eunmango | 97:b483e656bd14 | 230 | int readLine() { |
eunmango | 97:b483e656bd14 | 231 | unsigned char i, on_line =0; |
eunmango | 97:b483e656bd14 | 232 | unsigned long avg; |
eunmango | 97:b483e656bd14 | 233 | unsigned int sum; |
eunmango | 97:b483e656bd14 | 234 | static int last_value=0; |
eunmango | 97:b483e656bd14 | 235 | |
eunmango | 97:b483e656bd14 | 236 | readCalibrated(); |
eunmango | 97:b483e656bd14 | 237 | avg =0; sum =0; |
eunmango | 97:b483e656bd14 | 238 | for(i=0; i< NUMSENSORS; i++){ |
eunmango | 97:b483e656bd14 | 239 | int value = sensor_values[i]; |
eunmango | 97:b483e656bd14 | 240 | value = 1000-value; |
eunmango | 97:b483e656bd14 | 241 | sensor_values[i]=value; |
eunmango | 97:b483e656bd14 | 242 | in_line[i]=FALS; |
eunmango | 97:b483e656bd14 | 243 | if(value > 300) { |
eunmango | 97:b483e656bd14 | 244 | on_line =1; |
eunmango | 97:b483e656bd14 | 245 | in_line[i]=TRU; |
eunmango | 97:b483e656bd14 | 246 | } |
eunmango | 97:b483e656bd14 | 247 | //if(value > calibratedMin[i]) on_line=1; |
eunmango | 97:b483e656bd14 | 248 | if(value > 50){ |
eunmango | 97:b483e656bd14 | 249 | avg += (long)(value)*(i*1000); |
eunmango | 97:b483e656bd14 | 250 | sum += value; |
eunmango | 97:b483e656bd14 | 251 | } |
eunmango | 97:b483e656bd14 | 252 | } |
eunmango | 97:b483e656bd14 | 253 | |
eunmango | 97:b483e656bd14 | 254 | if(!on_line){ |
eunmango | 97:b483e656bd14 | 255 | if(last_value < (NUMSENSORS-1)*1000/2) return 0; |
eunmango | 97:b483e656bd14 | 256 | else return ( NUMSENSORS-1 ) * 1000; |
eunmango | 97:b483e656bd14 | 257 | } |
eunmango | 97:b483e656bd14 | 258 | |
eunmango | 97:b483e656bd14 | 259 | last_value = avg/sum; |
eunmango | 97:b483e656bd14 | 260 | |
eunmango | 97:b483e656bd14 | 261 | return last_value; |
eunmango | 97:b483e656bd14 | 262 | |
eunmango | 97:b483e656bd14 | 263 | } |
eunmango | 97:b483e656bd14 | 264 | |
eunmango | 97:b483e656bd14 | 265 | /**************************************************************** |
eunmango | 97:b483e656bd14 | 266 | before running, set up tlc1543, calibrate **********************/ |
eunmango | 97:b483e656bd14 | 267 | void settlc1543(){ |
eunmango | 97:b483e656bd14 | 268 | cs =1; |
eunmango | 97:b483e656bd14 | 269 | spi.format(16,0); |
eunmango | 97:b483e656bd14 | 270 | spi.frequency(2000000); |
eunmango | 97:b483e656bd14 | 271 | pc.printf("test\r\n"); |
eunmango | 97:b483e656bd14 | 272 | //pc.attach(callback(&rx_cb)); |
eunmango | 97:b483e656bd14 | 273 | |
eunmango | 97:b483e656bd14 | 274 | TRSensors(); |
eunmango | 97:b483e656bd14 | 275 | for(int i=0; i<10; i++) { |
eunmango | 97:b483e656bd14 | 276 | calibrate(); |
eunmango | 97:b483e656bd14 | 277 | wait(0.4); |
eunmango | 97:b483e656bd14 | 278 | } |
eunmango | 97:b483e656bd14 | 279 | // pc.printf("calibrate done\r\n"); |
eunmango | 97:b483e656bd14 | 280 | for(int i=0; i<NUMSENSORS; i++) { |
eunmango | 97:b483e656bd14 | 281 | // pc.printf("Calibration Min: %d Max: %d\r\n", calibratedMin[i], calibratedMax[i] ); |
eunmango | 97:b483e656bd14 | 282 | } |
eunmango | 97:b483e656bd14 | 283 | } |
eunmango | 97:b483e656bd14 | 284 | |
eunmango | 97:b483e656bd14 | 285 | |
eunmango | 97:b483e656bd14 | 286 | void rx_cb(void) { |
eunmango | 97:b483e656bd14 | 287 | |
eunmango | 97:b483e656bd14 | 288 | int ch; |
eunmango | 97:b483e656bd14 | 289 | ch = pc.getc(); |
eunmango | 97:b483e656bd14 | 290 | rx_buffer[index]=ch; |
eunmango | 97:b483e656bd14 | 291 | index++; |
eunmango | 97:b483e656bd14 | 292 | pc.putc(ch); |
eunmango | 97:b483e656bd14 | 293 | if(ch==0x0D) { |
eunmango | 97:b483e656bd14 | 294 | pc.putc(0x0A); |
eunmango | 97:b483e656bd14 | 295 | flag=1; |
eunmango | 97:b483e656bd14 | 296 | index=0; |
eunmango | 97:b483e656bd14 | 297 | } |
eunmango | 97:b483e656bd14 | 298 | |
eunmango | 97:b483e656bd14 | 299 | |
eunmango | 97:b483e656bd14 | 300 | } |
eunmango | 97:b483e656bd14 | 301 | |
eunmango | 97:b483e656bd14 | 302 | |
mbed_official | 82:abf1b1785bd7 | 303 | int main() |
mbed_official | 82:abf1b1785bd7 | 304 | { |
eunmango | 97:b483e656bd14 | 305 | clock_t t; |
eunmango | 97:b483e656bd14 | 306 | int f; |
eunmango | 97:b483e656bd14 | 307 | char displaySentence[40] = "Time is "; |
eunmango | 97:b483e656bd14 | 308 | char time[20]; |
eunmango | 97:b483e656bd14 | 309 | irrecv.enableIRIn(); |
eunmango | 97:b483e656bd14 | 310 | // mu.startUpdates();//start mesuring the distance |
eunmango | 97:b483e656bd14 | 311 | int data; |
eunmango | 97:b483e656bd14 | 312 | |
eunmango | 97:b483e656bd14 | 313 | SSD1306 display = SSD1306(); |
eunmango | 97:b483e656bd14 | 314 | |
eunmango | 97:b483e656bd14 | 315 | while(1){ |
eunmango | 97:b483e656bd14 | 316 | |
eunmango | 97:b483e656bd14 | 317 | if(irrecv.decode(&result)){ |
eunmango | 97:b483e656bd14 | 318 | pc.printf("%X\r\n",result.value); |
eunmango | 97:b483e656bd14 | 319 | irrecv.resume(); |
eunmango | 97:b483e656bd14 | 320 | |
eunmango | 97:b483e656bd14 | 321 | if(result.value == KEY1){ // 1 |
eunmango | 97:b483e656bd14 | 322 | break; |
eunmango | 97:b483e656bd14 | 323 | } |
eunmango | 97:b483e656bd14 | 324 | if(result.value == KEY2){ // 2 |
eunmango | 97:b483e656bd14 | 325 | IRForward(); |
eunmango | 97:b483e656bd14 | 326 | } |
eunmango | 97:b483e656bd14 | 327 | if(result.value == KEY3){ // 3 |
eunmango | 97:b483e656bd14 | 328 | CalibrationMotor(); |
eunmango | 97:b483e656bd14 | 329 | settlc1543(); |
eunmango | 97:b483e656bd14 | 330 | |
eunmango | 97:b483e656bd14 | 331 | } |
eunmango | 97:b483e656bd14 | 332 | if(result.value == KEY6){ // 6 |
eunmango | 97:b483e656bd14 | 333 | right(); |
mbed_official | 82:abf1b1785bd7 | 334 | |
eunmango | 97:b483e656bd14 | 335 | } |
eunmango | 97:b483e656bd14 | 336 | if(result.value == KEY4){ // 4 |
eunmango | 97:b483e656bd14 | 337 | left(); |
eunmango | 97:b483e656bd14 | 338 | } |
eunmango | 97:b483e656bd14 | 339 | if(result.value == KEY8 ){ // 8 |
eunmango | 97:b483e656bd14 | 340 | backward(); |
eunmango | 97:b483e656bd14 | 341 | } |
eunmango | 97:b483e656bd14 | 342 | if(result.value == KEY5){ // 5 |
eunmango | 97:b483e656bd14 | 343 | stop(); |
eunmango | 97:b483e656bd14 | 344 | } |
eunmango | 97:b483e656bd14 | 345 | } |
eunmango | 97:b483e656bd14 | 346 | |
eunmango | 97:b483e656bd14 | 347 | } |
eunmango | 97:b483e656bd14 | 348 | |
eunmango | 97:b483e656bd14 | 349 | |
eunmango | 97:b483e656bd14 | 350 | |
eunmango | 97:b483e656bd14 | 351 | t=clock(); |
eunmango | 97:b483e656bd14 | 352 | pc.printf("Hello PCF8574\n"); |
eunmango | 97:b483e656bd14 | 353 | while (1) { |
eunmango | 97:b483e656bd14 | 354 | |
eunmango | 97:b483e656bd14 | 355 | ultra_result = ultra.distance(); |
eunmango | 97:b483e656bd14 | 356 | if(ultra_result > 0 && ultra_result < 14) { |
eunmango | 97:b483e656bd14 | 357 | if(in_line[0]==TRU && in_line[1]==TRU ){ |
eunmango | 97:b483e656bd14 | 358 | stop(); |
eunmango | 97:b483e656bd14 | 359 | mode = 2; |
eunmango | 97:b483e656bd14 | 360 | } |
eunmango | 97:b483e656bd14 | 361 | |
eunmango | 97:b483e656bd14 | 362 | } |
eunmango | 97:b483e656bd14 | 363 | |
mbed_official | 82:abf1b1785bd7 | 364 | |
eunmango | 97:b483e656bd14 | 365 | if(mode == 1){ |
eunmango | 97:b483e656bd14 | 366 | pc.printf("enter\r\n"); |
eunmango | 97:b483e656bd14 | 367 | position = (float)readLine(); |
eunmango | 97:b483e656bd14 | 368 | pc.printf("position: %f\r\n", position); |
eunmango | 97:b483e656bd14 | 369 | float proportional =(float)(position-2000.0); //e(t) |
eunmango | 97:b483e656bd14 | 370 | float derivative = (float)(proportional - last_proportional); |
eunmango | 97:b483e656bd14 | 371 | integral += proportional; |
eunmango | 97:b483e656bd14 | 372 | last_proportional = proportional;// |
eunmango | 97:b483e656bd14 | 373 | //pid 15/60000/2 - 85% |
eunmango | 97:b483e656bd14 | 374 | float power_difference = proportional/16.0+ (float)integral/ |
eunmango | 97:b483e656bd14 | 375 | 60000.0+ derivative*2.0; //integral 변수 커지고 작아지는 값 확인하 |
eunmango | 97:b483e656bd14 | 376 | pc.printf("proportional : %f integral %f derivative : %f \r\n", proportional, integral, derivative); |
eunmango | 97:b483e656bd14 | 377 | const int maximum = 255; |
eunmango | 97:b483e656bd14 | 378 | const int base = 155; |
eunmango | 97:b483e656bd14 | 379 | pc.printf("power_difference is %f\r\n" , power_difference); |
eunmango | 97:b483e656bd14 | 380 | if(power_difference > base ) power_difference = base; |
eunmango | 97:b483e656bd14 | 381 | if(power_difference < -base) power_difference = -base; |
eunmango | 97:b483e656bd14 | 382 | |
eunmango | 97:b483e656bd14 | 383 | |
eunmango | 97:b483e656bd14 | 384 | |
eunmango | 97:b483e656bd14 | 385 | if(power_difference < 0){ //오른쪽으로 빠 |
eunmango | 97:b483e656bd14 | 386 | // rightmotor.setSpeed((float)(base + power_difference) / (float)maximum); |
eunmango | 97:b483e656bd14 | 387 | // leftmotor.setSpeed((float)base-power_difference/(float)maximum); |
eunmango | 97:b483e656bd14 | 388 | pc.printf("right : left %f, right %f \r\n",(float)(base+power_difference) / (float)maximum , (float)(base) / (float)maximum ); |
eunmango | 97:b483e656bd14 | 389 | rightmotor.setSpeed((float)(base) / (float)maximum); |
eunmango | 97:b483e656bd14 | 390 | leftmotor.setSpeed((float)(base+power_difference) / (float)maximum ); |
eunmango | 97:b483e656bd14 | 391 | } else{// 왼//쪽으로 빠짐. |
eunmango | 97:b483e656bd14 | 392 | // rightmotor.setSpeed((float)(base + power_difference) / (float)maximum); |
eunmango | 97:b483e656bd14 | 393 | // leftmotor.setSpeed((float)base-power_difference/(float)maximum); |
eunmango | 97:b483e656bd14 | 394 | pc.printf("left : left %f, right %f \r\n",(float)(base) / (float)maximum , (float)(base -power_difference) / (float)maximum ); |
eunmango | 97:b483e656bd14 | 395 | leftmotor.setSpeed((float)(base) / (float)maximum); |
eunmango | 97:b483e656bd14 | 396 | rightmotor.setSpeed((float)(base -power_difference) / (float)maximum); |
eunmango | 97:b483e656bd14 | 397 | |
eunmango | 97:b483e656bd14 | 398 | } |
eunmango | 97:b483e656bd14 | 399 | |
eunmango | 97:b483e656bd14 | 400 | if(in_line[0]&&in_line[1]&&in_line[2]&&in_line[3]&&in_line[4]){ |
eunmango | 97:b483e656bd14 | 401 | leftmotor.setSpeed(0); |
eunmango | 97:b483e656bd14 | 402 | rightmotor.setSpeed(0); |
eunmango | 97:b483e656bd14 | 403 | pc.printf("stop\r\n"); |
eunmango | 97:b483e656bd14 | 404 | //시간 모니터에 출력. |
eunmango | 97:b483e656bd14 | 405 | break; |
eunmango | 97:b483e656bd14 | 406 | } |
eunmango | 97:b483e656bd14 | 407 | } |
eunmango | 97:b483e656bd14 | 408 | |
eunmango | 97:b483e656bd14 | 409 | if(mode == 2){ |
eunmango | 97:b483e656bd14 | 410 | ultra_result = ultra.distance(); |
eunmango | 97:b483e656bd14 | 411 | |
eunmango | 97:b483e656bd14 | 412 | for(int i=0; i<50; i++){ |
eunmango | 97:b483e656bd14 | 413 | data = readLine(); |
eunmango | 97:b483e656bd14 | 414 | if(in_line[2]==TRU && in_line[1]==FALS && in_line[3]==FALS && in_line[0]==FALS && in_line[4]==FALS) |
eunmango | 97:b483e656bd14 | 415 | { |
eunmango | 97:b483e656bd14 | 416 | mode=1; |
eunmango | 97:b483e656bd14 | 417 | break; |
eunmango | 97:b483e656bd14 | 418 | } |
eunmango | 97:b483e656bd14 | 419 | ultraLeft(); |
eunmango | 97:b483e656bd14 | 420 | } |
eunmango | 97:b483e656bd14 | 421 | } |
Jonathan Austin |
0:2757d7abb7d9 | 422 | } |
eunmango | 97:b483e656bd14 | 423 | display.begin(true); |
eunmango | 97:b483e656bd14 | 424 | display.setTextSize(1); |
eunmango | 97:b483e656bd14 | 425 | display.setTextColor(WHITE); |
eunmango | 97:b483e656bd14 | 426 | display.setCursor(0,0); |
eunmango | 97:b483e656bd14 | 427 | t=clock()-t; |
eunmango | 97:b483e656bd14 | 428 | sprintf(time, "%f ms", (float)t*10); |
eunmango | 97:b483e656bd14 | 429 | strcat(displaySentence, time); |
eunmango | 97:b483e656bd14 | 430 | display.println(displaySentence); |
eunmango | 97:b483e656bd14 | 431 | display.display(); |
eunmango | 97:b483e656bd14 | 432 | } |