Final Project
Final Project로 실제 점검에 사용된 코드
Dependencies: mbed Adafruit_GFX
Diff: main.cpp
- Revision:
- 0:22391cd705e2
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sat Jun 15 20:52:15 2019 +0000
@@ -0,0 +1,404 @@
+#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
+#define button2 18
+
+// PWMA, AIN1, AIN2, PWMBm, BIN1, BIN2, STBY
+TB6612FNG motorDriver(D6, A0, A1, D5, A3, A2, A5);
+ReceiverIR ir_rx(D4);
+Serial pc(USBTX, USBRX);
+SPI spi(D11,D12,D13);
+DigitalOut spi_cs(D10,1);
+Ultrasonic ultra(D3, D2);
+PixelArray px(WS2812_BUF);
+WS2812 ws(D7, WS2812_BUF, 7, 15, 10, 15);
+
+I2C i2c(D14, D15);
+Adafruit_SSD1306_I2c oled(i2c, D9, 0x78, 64, 128);
+
+Timer timer;
+Timer btimer;
+int cal[5]; //센서 입력 모아놓은 배열
+int maxv = 0; //칼리브레이션 최대값
+int minv = 999; //칼리브레이션 최소값
+float fPwmPeriod; //모터 주기
+float fPwmPulsewidth; //모터 속도
+RemoteIR::Format format;
+uint8_t buf[32]; //리모컨 입력받는 버퍼
+int num; //리모컨 입력 숫자
+int bitcount, decoded; //리모컨 입력 필요변수
+
+float aspeed=0.19;
+float speed=0.19; //모터 속도
+float lspd=0.17; //최소 속도
+float mspd=0.25; //최대 속도
+float tspeed; //임시 속도 변
+
+void setspeed(float a){
+ speed=a;
+ motorDriver.setPwmApulsewidth(speed);
+ motorDriver.setPwmBpulsewidth(speed);
+}
+
+int going=0; //주행 중인지 아닌지 판별
+
+int maxcal(int a[], int k){
+ int q;
+ int maxone = 2;
+ int range = 150;
+ if(a[2]- a[0]>range) maxone = 0;
+ else if(a[2]- a[1]>range) maxone = 1;
+ else if(a[2]- a[3]>range) maxone = 3;
+ else if(a[2]- a[4]>range) maxone = 4;
+ int maxa2 = 250;
+ int mina2 = 100;
+ //if(timer.read() > 15 && maxa2 > a[0] && maxa2 > a[1] && mina2 < a[0] && mina2 < a[1]){
+// if(maxa2 > a[3] && mina2 < a[3] && maxa2 >a[4] && mina2 < a[4]){
+// maxone = 6;
+// }
+// maxone = 5;
+// }
+
+ return maxone;
+}
+
+void moveStop(void){
+ motorDriver.motorB_stop();
+ motorDriver.motorA_stop();
+}
+
+void moveBackward(void){
+ setspeed(speed);
+ motorDriver.motorA_ccw();
+ motorDriver.motorB_ccw();
+ //wait(0.3);
+ //moveStop();
+}
+
+void moveForward(void){
+ setspeed(speed);
+ motorDriver.motorA_cw();
+ motorDriver.motorB_cw();
+ //wait(0.3);
+ //moveStop();
+}
+
+void moveLeft(void){
+ setspeed(speed);
+ motorDriver.motorA_ccw();
+ motorDriver.motorB_cw();
+ wait(0.08);
+ moveStop();
+}
+
+void moveRight(void){
+ setspeed(speed);
+ motorDriver.motorA_cw();
+ motorDriver.motorB_ccw();
+ wait(0.08);
+ moveStop();
+}
+void finishlight(void)
+{
+ ws.useII(WS2812::PER_PIXEL); // use per-pixel intensity scaling
+
+ int colorbuf[NUM_COLORS] = {0x2f0000,0x2f2f00,0x002f00,0x002f2f,0x00002f,0x2f002f};
+
+ for (int i = 0; i < WS2812_BUF; i++) {
+ px.Set(i, colorbuf[(i / NUM_LEDS_PER_COLOR) % NUM_COLORS]);
+ }
+
+ for (int j=0; j<WS2812_BUF; j++) {
+ px.SetI(j%WS2812_BUF, 0xf+(0xf*(j%NUM_LEDS_PER_COLOR)));
+ }
+
+ while (1) {
+ for (int z=WS2812_BUF; z >= 0 ; z--) {
+ ws.write_offsets(px.getBuf(),z,z,z);
+ wait(0.075);
+ }
+ }
+}
+int value;
+int ch = 0;
+int status;
+void drive(void){
+ spi_cs = 0;
+ ultra.trig();
+ ultra.setMode(true);
+ value = spi.write(ch<<12);
+ pc.printf("calibration..\r\n");
+ timer.start();
+
+ //주행시작
+ motorDriver.setPwmApulsewidth(speed);
+ motorDriver.setPwmBpulsewidth(speed);
+ moveForward();
+ while(1){
+ oled.clearDisplay();
+ oled.setTextCursor(0,0);
+ oled.printf("speed: %f\r\n",speed);
+ oled.printf("0: %d, 1: %d\r\n2: %d, 3: %d\r\n4: %d st: %d\r\n",cal[0],cal[1],cal[2],cal[3],cal[4],status);
+ oled.printf("time: %.3f\r\n",timer.read_ms()/1000);
+ oled.display();
+
+ status = maxcal(cal,5);
+ pc.printf("status:%d\r\n");
+ switch(status){
+ //●○○○○, 오른쪽으로 가야함
+ case 4 :
+ setspeed(aspeed);
+ motorDriver.motorA_cw();
+ motorDriver.motorB_ccw();
+ wait(0.08);
+ moveStop();
+ wait(0.08);
+ break;
+
+ //○●○○○, 오른쪽으로 가야함
+ case 3 :
+ setspeed(aspeed);
+ motorDriver.motorA_cw();
+ motorDriver.motorB_ccw();
+ wait(0.08);
+ moveStop();
+ wait(0.08);
+ break;
+
+ //○○●○○, 정주행
+ // 중앙 값이 가장 낮지만 검은 줄이 아니면 (<350) 뒤로 0.5초간 가본다. 그 뒤에 다시 직진.
+ case 2 :
+ speed=aspeed;
+ if(speed<mspd){speed+=0.02;}
+ motorDriver.setPwmApulsewidth(speed);
+ motorDriver.setPwmBpulsewidth(speed);
+ if(cal[2]<350 || btimer.read_ms()>500){moveForward();btimer.reset();btimer.start();}
+ else{
+ if(btimer.read_ms()<500){
+ moveBackward();
+ btimer.reset();
+ btimer.start();
+ }
+
+ }
+
+ break;
+
+ //○○○●○, 왼쪽으로 가야함
+ case 1 :
+ setspeed(aspeed);
+ motorDriver.motorA_ccw();
+ motorDriver.motorB_cw();
+ wait(0.08);
+ moveStop();
+ wait(0.08);
+ break;
+
+ //○○○○●, 왼쪽으로 가야함
+ case 0 :
+ setspeed(aspeed);
+ motorDriver.motorA_ccw();
+ motorDriver.motorB_cw();
+ wait(0.08);
+ moveStop();
+ wait(0.08);
+ break;
+
+// case 5 :
+// speed=1.5;
+// motorDriver.setPwmApulsewidth(speed);
+// motorDriver.setPwmBpulsewidth(speed);
+// moveLeft();
+// moveForward();
+// break;
+
+ case 6 :
+ moveStop();
+ finishlight();
+
+ default :
+ motorDriver.setPwmApulsewidth(speed);
+ motorDriver.setPwmBpulsewidth(speed);
+ }
+
+ //센서 입력 받기
+ 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);
+ switch(ch)
+ {
+ case 1 : cal[0] = value;break;
+ case 2 : cal[1] = value;break;
+ case 3 : cal[2] = value;break;
+ case 4 : cal[3] = value;break;
+ case 5 : cal[4] = value;break;
+ }
+ }
+
+ //각종 프린트
+ //if(timer.read()>1){
+ for(ch=0;ch<5;ch++){
+ pc.printf("cal[%d]: %d, ",ch, cal[ch]);
+ if(cal[ch] < minv) minv = cal[ch];
+ if(cal[ch] > maxv) maxv = cal[ch];
+ }
+ pc.printf("\r\n");
+ pc.printf("maxv: %d, minv: %d,\r\n",maxv, minv);
+ pc.printf("maxcal : %d\r\n",status);
+ pc.printf("\r\n");
+ //}
+
+ //장애물 만나면 스탑!!
+ if(ultra.getDistance()<8)
+ {
+ pc.printf("WARNING!\r\n");
+ tspeed = speed;
+ speed=0.2;
+ motorDriver.setPwmApulsewidth(speed);
+ motorDriver.setPwmBpulsewidth(speed);
+ moveBackward();
+ wait(0.2);
+ moveStop();
+ speed=tspeed;
+ motorDriver.setPwmApulsewidth(speed);
+ motorDriver.setPwmBpulsewidth(speed);
+ }
+
+ //리모컨 입력받기
+ if (ir_rx.getState() == ReceiverIR::Received) {
+ bitcount = ir_rx.getData(&format, buf, sizeof(buf) * 8);
+ if (bitcount>0) {
+ decoded=buf[3];
+ num = buf[2];
+ //pc.printf("\r\nDecoded: %X %X %X %X ", buf[0],buf[1],num,buf[3]);
+ pc.printf("\r\nnum: %X %d", num, num);
+ switch(num){
+ case 24:
+ moveForward();
+ break;
+ case 90:
+ moveRight();
+ break;
+ case 82:
+ moveBackward();
+ break;
+ case 8:
+ moveLeft();
+ break;
+ case 21: //속도상승
+ speed+=0.05;
+ motorDriver.setPwmApulsewidth(speed);
+ motorDriver.setPwmBpulsewidth(speed);
+ break;
+ case 7: //속도저하
+ speed-=0.05;
+ motorDriver.setPwmApulsewidth(speed);
+ motorDriver.setPwmBpulsewidth(speed);
+ break;
+ case 28:
+ pc.printf("stop");
+ moveStop();
+ going=0;
+ break;
+ case 68:
+ pc.printf("stop drive\n\r");
+ going=0;
+ break;
+ }
+ }
+ }
+ }
+}
+
+/*
+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()
+{
+ pc.printf("HEllO\r\n\r\n");
+ pc.printf("maxv: %d, minv: %d,\r\n",maxv, minv);
+
+ while(1) {
+ if (ir_rx.getState() == ReceiverIR::Received) {
+ bitcount = ir_rx.getData(&format, buf, sizeof(buf) * 8);
+ if (bitcount>0) {
+ decoded=buf[3];
+ num = buf[2];
+ //pc.printf("\r\nDecoded: %X %X %X %X ", buf[0],buf[1],num,buf[3]);
+ pc.printf("\r\nnum: %X %d", num, num);
+ switch(num){
+ case 24:
+ moveForward();
+ break;
+ case 90:
+ moveRight();
+ break;
+ case 82:
+ moveBackward();
+ break;
+ case 8:
+ moveLeft();
+ break;
+ case 21: //속도상승
+ speed+=0.1;
+ motorDriver.setPwmApulsewidth(speed);
+ motorDriver.setPwmBpulsewidth(speed);
+ break;
+ case 7: //속도저하
+ speed-=0.1;
+ motorDriver.setPwmApulsewidth(speed);
+ motorDriver.setPwmBpulsewidth(speed);
+ break;
+ case 28:
+ pc.printf("stop");
+ moveStop();
+ going=0;
+ break;
+ case 64:
+ pc.printf("drive\n\r");
+ going=1;
+ timer.reset();
+ drive();
+ break;
+ case 68:
+ pc.printf("stop drive\n\r");
+ going=0;
+ break;
+ }
+ }
+ }
+ }
+}