Diff: main.cpp

Revision:

0:c4c874d702f9

diff -r 000000000000 -r c4c874d702f9 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Jun 15 20:39:39 2019 +0000
@@ -0,0 +1,269 @@
+#include "mbed.h"
+#include "ReceiverIR.h"
+#include "TB6612FNG.h"
+#include "Ultrasonic.h"
+
+#include "Adafruit_SSD1306.h"
+#include "Adafruit_GFX.h"
+#include "glcdfont.h"
+#define SSD1306_DISPLAYON 0xAF
+#include "WS2812.h"
+#include "PixelArray.h"
+ 
+#define WS2812_BUF 150
+#define NUM_COLORS 6
+#define NUM_LEDS_PER_COLOR 10
+ 
+PixelArray px(WS2812_BUF);
+
+#define button2 18
+
+TB6612FNG motorDriver(D6, A0, A1, D5, A3, A2, A5);
+ReceiverIR ir_rx(D4);
+SPI spi(D11,D12,D13);
+DigitalOut spi_cs(D10,1);
+Ultrasonic ultra(D3, D2);
+WS2812 ws(D7, WS2812_BUF, 7, 15, 10, 15);
+
+I2C i2c(D14, D15);
+Adafruit_SSD1306_I2c oled(i2c, D9, 0x78, 64, 128);
+
+float fPwmPeriod; //모터 주기
+float fPwmPulsewidth; //모터 속도
+RemoteIR::Format format;
+uint8_t buf[32]; //리모컨 입력받는 버퍼
+int num; //리모컨 입력 숫자
+int bitcount, decoded; //리모컨 입력 필요변수
+
+void moveStop(void){
+    motorDriver.motorB_stop();
+    motorDriver.motorA_stop();
+}
+
+void moveBackward(void){
+    motorDriver.motorA_ccw();
+    motorDriver.motorB_ccw();
+}
+
+void moveForward(void){
+    motorDriver.motorA_cw();
+    motorDriver.motorB_cw();
+}
+
+void moveLeft(void){
+    motorDriver.motorA_ccw();
+    motorDriver.motorB_cw();
+    wait(0.1);
+    moveStop();
+}
+
+void moveRight(void){
+  motorDriver.motorA_cw();
+  motorDriver.motorB_ccw();
+  wait(0.1);
+  moveStop();
+}
+
+Timer timer;
+int value; //센서 읽은 값
+int ch = 0; //센서 채널 번호
+
+int sensors[5]; //센서 입력 모아놓은 배열
+int sensormin[5] = {999}; //센서별 최소값
+int sensormax[5] = {0}; //센서별 최대값
+int sensorres[5] = {1}; //센서별 범위값
+int sensors_sum=0;
+int sensors_average=0;
+int position = 0;
+int proportional = 0;
+int last_proportional = 0;
+int derivative = 0;
+int setpoint = 0;
+
+float error_value = 0;
+float max_speed = 0.2;
+float right_speed = 0;
+float left_speed = 0;
+float Kp=0.001;
+float Kd=0.001;
+
+void readvalues(void){
+  for(ch=0;ch<6;ch++){
+    spi_cs = 0;
+    spi.format(16, 0);
+    spi.frequency(2000000);
+    wait_us(2);
+    value = spi.write(ch<<12);
+    value >>= 6;
+    spi_cs = 1;
+    wait_us(21);
+    if(ch>0){
+      if(sensormin[ch-1] > value){sensormin[ch-1] = value;}
+      if(sensormax[ch-1] < value){sensormax[ch-1] = value;}
+    }
+  }
+}
+
+void calibration(void){
+  timer.reset();
+  oled.clearDisplay();
+  oled.setTextCursor(0,0);
+  oled.printf("Calibration Start!\r\n");
+  timer.start();
+  while(timer.read_ms()<5000){
+    moveLeft();
+    readvalues();
+    wait(0.1);
+  }
+
+  oled.clearDisplay();
+  oled.setTextCursor(0,0);
+  oled.printf("!! Calibration Result:\r\n");
+
+  for(ch=0;ch<5;ch++){
+    sensorres[ch] = sensormax[ch] - sensormin[ch];
+    oled.printf("%d: %d, ",ch,sensorres[ch]);
+    if(ch==1 || ch==3) {oled.printf("\r\n");}
+    oled.display();
+  }
+  wait(1);
+}
+
+void readcalibrated(void){
+  for(ch=0;ch<6;ch++){
+    spi_cs = 0;
+    spi.format(16, 0);
+    spi.frequency(2000000);
+    wait_us(2);
+    value = spi.write(ch<<12);
+    value >>= 6;
+    spi_cs = 1;
+    wait_us(21);
+    if(ch>0){
+      sensors[ch-1]=value/sensorres[ch-1] * 1000;
+      sensors_average += sensors[ch-1] * ch * 1000;
+      sensors_sum += sensors[ch-1];
+    }
+  }
+}
+
+void pid_calc(void){
+  position = int(sensors_average / sensors_sum);
+  proportional = position - setpoint;
+  derivative = proportional - last_proportional;
+  last_proportional = proportional;
+  error_value = float(proportional * Kp + derivative * Kd);
+}
+
+void calc_turn(void){
+  if(error_value < -0.1){
+    error_value = -0.1;
+  }
+  if(error_value > 0.1){
+    error_value = 0.1;
+  }
+  if(error_value < 0){
+    right_speed = max_speed + error_value;
+    left_speed = max_speed;
+  }
+  else{
+    right_speed = max_speed;
+    left_speed = max_speed - error_value;
+  }
+}
+
+void motor_drive(void){
+  motorDriver.setPwmApulsewidth(left_speed);
+  motorDriver.setPwmBpulsewidth(right_speed);
+  moveForward();
+}
+
+void loop(void){
+  readcalibrated();
+  pid_calc();
+  calc_turn();
+  motor_drive();
+}
+
+/*
+1 = 12
+2 = 24
+3 = 94
+4 = 8
+5 = 28
+6 = 90
+7 = 66
+8 = 82
+9 = 74
+0 = 22
++100 = 25
++200 = 13
+next = 64
+prev = 68
+vol+ = 21
+vol- = 7
+*/
+int main() {
+    spi_cs = 0; //SPI
+    ultra.trig(); //울트라 소닉 시작
+    ultra.setMode(true); //울트라 소닉 측정시작
+
+  while(1){
+      oled.clearDisplay();
+      oled.setTextCursor(0,0);
+      oled.printf("ls: %f ",left_speed);
+      oled.printf("rs: %f\r\n",right_speed);
+      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]);
+      //oled.printf("maxv: %d, minv: %d\r\n",maxv, minv);
+      oled.display();
+
+      readcalibrated();
+      pid_calc();
+
+      //리모컨 입력받기
+      if (ir_rx.getState() == ReceiverIR::Received) {
+          bitcount = ir_rx.getData(&format, buf, sizeof(buf) * 8);
+          if (bitcount>0) {
+              decoded=buf[3];
+              num = buf[2];
+              switch(num){
+                  case 24:
+                      moveForward();
+                      break;
+                  case 90:
+                      moveRight();
+                      break;
+                  case 82:
+                      moveBackward();
+                      break;
+                  case 8:
+                      moveLeft();
+                      break;
+                  case 21: //속도상승
+                      max_speed+=0.05;
+                      break;
+                  case 7: //속도저하
+                      max_speed-=-0.05;
+                      break;
+                  case 28:
+                      moveStop();
+                      break;
+                  case 25:
+                      calibration();
+                      break;
+                  case 13:
+                      loop();
+                      break;
+              }
+          }
+      }
+
+      //장애물 만나면 스탑!!
+      if(ultra.getDistance()<10)
+      {
+          moveBackward();
+          wait(0.01);
+          moveStop();
+      }
+  }
+}