Final Project / Mbed 2 deprecated Remote-Control_PID

나중에 급하게 PID 알고리즘을 적용해 짜본 코드... 시간이 충분치 않아서 그냥 원래 있던 코드를 수정해서 하기로 했기에 버려진 코드지만, 교수님께 참고용으로 Publish를 했다.

Dependencies:   mbed Adafruit_GFX

main.cpp@0:c4c874d702f9, 2019-06-15 (annotated)

Committer:
21400688
Date:
Sat Jun 15 20:39:39 2019 +0000
Revision:
0:c4c874d702f9
first commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
21400688 0:c4c874d702f9 1 #include "mbed.h"
21400688 0:c4c874d702f9 2 #include "ReceiverIR.h"
21400688 0:c4c874d702f9 3 #include "TB6612FNG.h"
21400688 0:c4c874d702f9 4 #include "Ultrasonic.h"
21400688 0:c4c874d702f9 5
21400688 0:c4c874d702f9 6 #include "Adafruit_SSD1306.h"
21400688 0:c4c874d702f9 7 #include "Adafruit_GFX.h"
21400688 0:c4c874d702f9 8 #include "glcdfont.h"
21400688 0:c4c874d702f9 9 #define SSD1306_DISPLAYON 0xAF
21400688 0:c4c874d702f9 10 #include "WS2812.h"
21400688 0:c4c874d702f9 11 #include "PixelArray.h"
21400688 0:c4c874d702f9 12
21400688 0:c4c874d702f9 13 #define WS2812_BUF 150
21400688 0:c4c874d702f9 14 #define NUM_COLORS 6
21400688 0:c4c874d702f9 15 #define NUM_LEDS_PER_COLOR 10
21400688 0:c4c874d702f9 16
21400688 0:c4c874d702f9 17 PixelArray px(WS2812_BUF);
21400688 0:c4c874d702f9 18
21400688 0:c4c874d702f9 19 #define button2 18
21400688 0:c4c874d702f9 20
21400688 0:c4c874d702f9 21 TB6612FNG motorDriver(D6, A0, A1, D5, A3, A2, A5);
21400688 0:c4c874d702f9 22 ReceiverIR ir_rx(D4);
21400688 0:c4c874d702f9 23 SPI spi(D11,D12,D13);
21400688 0:c4c874d702f9 24 DigitalOut spi_cs(D10,1);
21400688 0:c4c874d702f9 25 Ultrasonic ultra(D3, D2);
21400688 0:c4c874d702f9 26 WS2812 ws(D7, WS2812_BUF, 7, 15, 10, 15);
21400688 0:c4c874d702f9 27
21400688 0:c4c874d702f9 28 I2C i2c(D14, D15);
21400688 0:c4c874d702f9 29 Adafruit_SSD1306_I2c oled(i2c, D9, 0x78, 64, 128);
21400688 0:c4c874d702f9 30
21400688 0:c4c874d702f9 31 float fPwmPeriod; //모터 주기
21400688 0:c4c874d702f9 32 float fPwmPulsewidth; //모터 속도
21400688 0:c4c874d702f9 33 RemoteIR::Format format;
21400688 0:c4c874d702f9 34 uint8_t buf[32]; //리모컨 입력받는 버퍼
21400688 0:c4c874d702f9 35 int num; //리모컨 입력 숫자
21400688 0:c4c874d702f9 36 int bitcount, decoded; //리모컨 입력 필요변수
21400688 0:c4c874d702f9 37
21400688 0:c4c874d702f9 38 void moveStop(void){
21400688 0:c4c874d702f9 39 motorDriver.motorB_stop();
21400688 0:c4c874d702f9 40 motorDriver.motorA_stop();
21400688 0:c4c874d702f9 41 }
21400688 0:c4c874d702f9 42
21400688 0:c4c874d702f9 43 void moveBackward(void){
21400688 0:c4c874d702f9 44 motorDriver.motorA_ccw();
21400688 0:c4c874d702f9 45 motorDriver.motorB_ccw();
21400688 0:c4c874d702f9 46 }
21400688 0:c4c874d702f9 47
21400688 0:c4c874d702f9 48 void moveForward(void){
21400688 0:c4c874d702f9 49 motorDriver.motorA_cw();
21400688 0:c4c874d702f9 50 motorDriver.motorB_cw();
21400688 0:c4c874d702f9 51 }
21400688 0:c4c874d702f9 52
21400688 0:c4c874d702f9 53 void moveLeft(void){
21400688 0:c4c874d702f9 54 motorDriver.motorA_ccw();
21400688 0:c4c874d702f9 55 motorDriver.motorB_cw();
21400688 0:c4c874d702f9 56 wait(0.1);
21400688 0:c4c874d702f9 57 moveStop();
21400688 0:c4c874d702f9 58 }
21400688 0:c4c874d702f9 59
21400688 0:c4c874d702f9 60 void moveRight(void){
21400688 0:c4c874d702f9 61 motorDriver.motorA_cw();
21400688 0:c4c874d702f9 62 motorDriver.motorB_ccw();
21400688 0:c4c874d702f9 63 wait(0.1);
21400688 0:c4c874d702f9 64 moveStop();
21400688 0:c4c874d702f9 65 }
21400688 0:c4c874d702f9 66
21400688 0:c4c874d702f9 67 Timer timer;
21400688 0:c4c874d702f9 68 int value; //센서 읽은 값
21400688 0:c4c874d702f9 69 int ch = 0; //센서 채널 번호
21400688 0:c4c874d702f9 70
21400688 0:c4c874d702f9 71 int sensors[5]; //센서 입력 모아놓은 배열
21400688 0:c4c874d702f9 72 int sensormin[5] = {999}; //센서별 최소값
21400688 0:c4c874d702f9 73 int sensormax[5] = {0}; //센서별 최대값
21400688 0:c4c874d702f9 74 int sensorres[5] = {1}; //센서별 범위값
21400688 0:c4c874d702f9 75 int sensors_sum=0;
21400688 0:c4c874d702f9 76 int sensors_average=0;
21400688 0:c4c874d702f9 77 int position = 0;
21400688 0:c4c874d702f9 78 int proportional = 0;
21400688 0:c4c874d702f9 79 int last_proportional = 0;
21400688 0:c4c874d702f9 80 int derivative = 0;
21400688 0:c4c874d702f9 81 int setpoint = 0;
21400688 0:c4c874d702f9 82
21400688 0:c4c874d702f9 83 float error_value = 0;
21400688 0:c4c874d702f9 84 float max_speed = 0.2;
21400688 0:c4c874d702f9 85 float right_speed = 0;
21400688 0:c4c874d702f9 86 float left_speed = 0;
21400688 0:c4c874d702f9 87 float Kp=0.001;
21400688 0:c4c874d702f9 88 float Kd=0.001;
21400688 0:c4c874d702f9 89
21400688 0:c4c874d702f9 90 void readvalues(void){
21400688 0:c4c874d702f9 91 for(ch=0;ch<6;ch++){
21400688 0:c4c874d702f9 92 spi_cs = 0;
21400688 0:c4c874d702f9 93 spi.format(16, 0);
21400688 0:c4c874d702f9 94 spi.frequency(2000000);
21400688 0:c4c874d702f9 95 wait_us(2);
21400688 0:c4c874d702f9 96 value = spi.write(ch<<12);
21400688 0:c4c874d702f9 97 value >>= 6;
21400688 0:c4c874d702f9 98 spi_cs = 1;
21400688 0:c4c874d702f9 99 wait_us(21);
21400688 0:c4c874d702f9 100 if(ch>0){
21400688 0:c4c874d702f9 101 if(sensormin[ch-1] > value){sensormin[ch-1] = value;}
21400688 0:c4c874d702f9 102 if(sensormax[ch-1] < value){sensormax[ch-1] = value;}
21400688 0:c4c874d702f9 103 }
21400688 0:c4c874d702f9 104 }
21400688 0:c4c874d702f9 105 }
21400688 0:c4c874d702f9 106
21400688 0:c4c874d702f9 107 void calibration(void){
21400688 0:c4c874d702f9 108 timer.reset();
21400688 0:c4c874d702f9 109 oled.clearDisplay();
21400688 0:c4c874d702f9 110 oled.setTextCursor(0,0);
21400688 0:c4c874d702f9 111 oled.printf("Calibration Start!\r\n");
21400688 0:c4c874d702f9 112 timer.start();
21400688 0:c4c874d702f9 113 while(timer.read_ms()<5000){
21400688 0:c4c874d702f9 114 moveLeft();
21400688 0:c4c874d702f9 115 readvalues();
21400688 0:c4c874d702f9 116 wait(0.1);
21400688 0:c4c874d702f9 117 }
21400688 0:c4c874d702f9 118
21400688 0:c4c874d702f9 119 oled.clearDisplay();
21400688 0:c4c874d702f9 120 oled.setTextCursor(0,0);
21400688 0:c4c874d702f9 121 oled.printf("!! Calibration Result:\r\n");
21400688 0:c4c874d702f9 122
21400688 0:c4c874d702f9 123 for(ch=0;ch<5;ch++){
21400688 0:c4c874d702f9 124 sensorres[ch] = sensormax[ch] - sensormin[ch];
21400688 0:c4c874d702f9 125 oled.printf("%d: %d, ",ch,sensorres[ch]);
21400688 0:c4c874d702f9 126 if(ch==1 || ch==3) {oled.printf("\r\n");}
21400688 0:c4c874d702f9 127 oled.display();
21400688 0:c4c874d702f9 128 }
21400688 0:c4c874d702f9 129 wait(1);
21400688 0:c4c874d702f9 130 }
21400688 0:c4c874d702f9 131
21400688 0:c4c874d702f9 132 void readcalibrated(void){
21400688 0:c4c874d702f9 133 for(ch=0;ch<6;ch++){
21400688 0:c4c874d702f9 134 spi_cs = 0;
21400688 0:c4c874d702f9 135 spi.format(16, 0);
21400688 0:c4c874d702f9 136 spi.frequency(2000000);
21400688 0:c4c874d702f9 137 wait_us(2);
21400688 0:c4c874d702f9 138 value = spi.write(ch<<12);
21400688 0:c4c874d702f9 139 value >>= 6;
21400688 0:c4c874d702f9 140 spi_cs = 1;
21400688 0:c4c874d702f9 141 wait_us(21);
21400688 0:c4c874d702f9 142 if(ch>0){
21400688 0:c4c874d702f9 143 sensors[ch-1]=value/sensorres[ch-1] * 1000;
21400688 0:c4c874d702f9 144 sensors_average += sensors[ch-1] * ch * 1000;
21400688 0:c4c874d702f9 145 sensors_sum += sensors[ch-1];
21400688 0:c4c874d702f9 146 }
21400688 0:c4c874d702f9 147 }
21400688 0:c4c874d702f9 148 }
21400688 0:c4c874d702f9 149
21400688 0:c4c874d702f9 150 void pid_calc(void){
21400688 0:c4c874d702f9 151 position = int(sensors_average / sensors_sum);
21400688 0:c4c874d702f9 152 proportional = position - setpoint;
21400688 0:c4c874d702f9 153 derivative = proportional - last_proportional;
21400688 0:c4c874d702f9 154 last_proportional = proportional;
21400688 0:c4c874d702f9 155 error_value = float(proportional * Kp + derivative * Kd);
21400688 0:c4c874d702f9 156 }
21400688 0:c4c874d702f9 157
21400688 0:c4c874d702f9 158 void calc_turn(void){
21400688 0:c4c874d702f9 159 if(error_value < -0.1){
21400688 0:c4c874d702f9 160 error_value = -0.1;
21400688 0:c4c874d702f9 161 }
21400688 0:c4c874d702f9 162 if(error_value > 0.1){
21400688 0:c4c874d702f9 163 error_value = 0.1;
21400688 0:c4c874d702f9 164 }
21400688 0:c4c874d702f9 165 if(error_value < 0){
21400688 0:c4c874d702f9 166 right_speed = max_speed + error_value;
21400688 0:c4c874d702f9 167 left_speed = max_speed;
21400688 0:c4c874d702f9 168 }
21400688 0:c4c874d702f9 169 else{
21400688 0:c4c874d702f9 170 right_speed = max_speed;
21400688 0:c4c874d702f9 171 left_speed = max_speed - error_value;
21400688 0:c4c874d702f9 172 }
21400688 0:c4c874d702f9 173 }
21400688 0:c4c874d702f9 174
21400688 0:c4c874d702f9 175 void motor_drive(void){
21400688 0:c4c874d702f9 176 motorDriver.setPwmApulsewidth(left_speed);
21400688 0:c4c874d702f9 177 motorDriver.setPwmBpulsewidth(right_speed);
21400688 0:c4c874d702f9 178 moveForward();
21400688 0:c4c874d702f9 179 }
21400688 0:c4c874d702f9 180
21400688 0:c4c874d702f9 181 void loop(void){
21400688 0:c4c874d702f9 182 readcalibrated();
21400688 0:c4c874d702f9 183 pid_calc();
21400688 0:c4c874d702f9 184 calc_turn();
21400688 0:c4c874d702f9 185 motor_drive();
21400688 0:c4c874d702f9 186 }
21400688 0:c4c874d702f9 187
21400688 0:c4c874d702f9 188 /*
21400688 0:c4c874d702f9 189 1 = 12
21400688 0:c4c874d702f9 190 2 = 24
21400688 0:c4c874d702f9 191 3 = 94
21400688 0:c4c874d702f9 192 4 = 8
21400688 0:c4c874d702f9 193 5 = 28
21400688 0:c4c874d702f9 194 6 = 90
21400688 0:c4c874d702f9 195 7 = 66
21400688 0:c4c874d702f9 196 8 = 82
21400688 0:c4c874d702f9 197 9 = 74
21400688 0:c4c874d702f9 198 0 = 22
21400688 0:c4c874d702f9 199 +100 = 25
21400688 0:c4c874d702f9 200 +200 = 13
21400688 0:c4c874d702f9 201 next = 64
21400688 0:c4c874d702f9 202 prev = 68
21400688 0:c4c874d702f9 203 vol+ = 21
21400688 0:c4c874d702f9 204 vol- = 7
21400688 0:c4c874d702f9 205 */
21400688 0:c4c874d702f9 206 int main() {
21400688 0:c4c874d702f9 207 spi_cs = 0; //SPI
21400688 0:c4c874d702f9 208 ultra.trig(); //울트라 소닉 시작
21400688 0:c4c874d702f9 209 ultra.setMode(true); //울트라 소닉 측정시작
21400688 0:c4c874d702f9 210
21400688 0:c4c874d702f9 211 while(1){
21400688 0:c4c874d702f9 212 oled.clearDisplay();
21400688 0:c4c874d702f9 213 oled.setTextCursor(0,0);
21400688 0:c4c874d702f9 214 oled.printf("ls: %f ",left_speed);
21400688 0:c4c874d702f9 215 oled.printf("rs: %f\r\n",right_speed);
21400688 0:c4c874d702f9 216 oled.printf("0: %d, 1: %d\r\n2: %d, 3: %d\r\n4: %d\r\n",sensors[0],sensors[1],sensors[2],sensors[3],sensors[4]);
21400688 0:c4c874d702f9 217 //oled.printf("maxv: %d, minv: %d\r\n",maxv, minv);
21400688 0:c4c874d702f9 218 oled.display();
21400688 0:c4c874d702f9 219
21400688 0:c4c874d702f9 220 readcalibrated();
21400688 0:c4c874d702f9 221 pid_calc();
21400688 0:c4c874d702f9 222
21400688 0:c4c874d702f9 223 //리모컨 입력받기
21400688 0:c4c874d702f9 224 if (ir_rx.getState() == ReceiverIR::Received) {
21400688 0:c4c874d702f9 225 bitcount = ir_rx.getData(&format, buf, sizeof(buf) * 8);
21400688 0:c4c874d702f9 226 if (bitcount>0) {
21400688 0:c4c874d702f9 227 decoded=buf[3];
21400688 0:c4c874d702f9 228 num = buf[2];
21400688 0:c4c874d702f9 229 switch(num){
21400688 0:c4c874d702f9 230 case 24:
21400688 0:c4c874d702f9 231 moveForward();
21400688 0:c4c874d702f9 232 break;
21400688 0:c4c874d702f9 233 case 90:
21400688 0:c4c874d702f9 234 moveRight();
21400688 0:c4c874d702f9 235 break;
21400688 0:c4c874d702f9 236 case 82:
21400688 0:c4c874d702f9 237 moveBackward();
21400688 0:c4c874d702f9 238 break;
21400688 0:c4c874d702f9 239 case 8:
21400688 0:c4c874d702f9 240 moveLeft();
21400688 0:c4c874d702f9 241 break;
21400688 0:c4c874d702f9 242 case 21: //속도상승
21400688 0:c4c874d702f9 243 max_speed+=0.05;
21400688 0:c4c874d702f9 244 break;
21400688 0:c4c874d702f9 245 case 7: //속도저하
21400688 0:c4c874d702f9 246 max_speed-=-0.05;
21400688 0:c4c874d702f9 247 break;
21400688 0:c4c874d702f9 248 case 28:
21400688 0:c4c874d702f9 249 moveStop();
21400688 0:c4c874d702f9 250 break;
21400688 0:c4c874d702f9 251 case 25:
21400688 0:c4c874d702f9 252 calibration();
21400688 0:c4c874d702f9 253 break;
21400688 0:c4c874d702f9 254 case 13:
21400688 0:c4c874d702f9 255 loop();
21400688 0:c4c874d702f9 256 break;
21400688 0:c4c874d702f9 257 }
21400688 0:c4c874d702f9 258 }
21400688 0:c4c874d702f9 259 }
21400688 0:c4c874d702f9 260
21400688 0:c4c874d702f9 261 //장애물 만나면 스탑!!
21400688 0:c4c874d702f9 262 if(ultra.getDistance()<10)
21400688 0:c4c874d702f9 263 {
21400688 0:c4c874d702f9 264 moveBackward();
21400688 0:c4c874d702f9 265 wait(0.01);
21400688 0:c4c874d702f9 266 moveStop();
21400688 0:c4c874d702f9 267 }
21400688 0:c4c874d702f9 268 }
21400688 0:c4c874d702f9 269 }