File content as of revision 0:22391cd705e2:

#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;
                }
            }
        } 
    }
}