查看: 14919|回复: 8

走钢丝+扫雷的巡迹小车

本帖最后由 virtualwiz 于 2016-9-20 20:48 编辑

巡迹车估计是入门Arduino的同学们玩厌了的项目了吧

只要在地上贴个黑线，借助光学传感器和一些控制手段，就可以让小车（或者高大上一些的，Vortex）一路沿着黑线行驶。

简单点的做法，只要用红外反射传感器，电压比较器和几片逻辑器件就可以晃晃悠悠起来
复杂点的做法，可以用一大堆光学传感器，在高性能的单片机上运行PID算法，做出可以竞速的智能车

这回我们来做个不一样的巡迹车~~

这个小车用到了DFRobot的Romeo BLE控制器，板载蓝牙和电机驱动，做这种项目灰常合适

小车启动后，沿着一根直径1mm的细铁丝慢慢前进。轨道的随机位置会出现一枚硬币，小车巡迹途中如果遇到硬币，会发出报警声。
（参加过某比赛的同学们别笑我:lol）
走钢丝+扫雷的巡迹小车图7

因为要检测的是细铁丝，可以使用TI公司的LDC1000数字电感传感器。

粗略地说，原理大概就是，
让一个空心的线圈靠近导体，这时导体就充当了线圈的铁芯，线圈的电感就会略微增加。LDC传感器以很高的频率扫描线圈，当电感特性发生了变化，就说明有导体靠近线圈，这就测出了我们需要的信息。

然后找个舵机，把线圈固定在一个杆上，让舵机来回扫动，同时不断读取LDC传感器的数据。这样，哪里测得的信号最强，就是细铁丝最有可能出现的方向，根据这个数值调整车的方向就可以了

这是舵机扫描过程中用串口绘图器画出的波形，上方峰值的位置就是最可能出现铁丝的位置。

Romeo上的电机驱动跳线使用了一种不同寻常的接法，将右上方的跳线交叉相连，将PWM信号从L298N的DIR引脚送入，经过测试，这种接法可以显著提升直流电机的低转速性能，但是同时会增加驱动芯片和电机的发热量。

送上代码~~据说写代码要规范，别人看到才会舒服。

<font face="Verdana">

//3rd version

#include <Servo.h>
#include "SPI.h"
#include <LiquidCrystal_I2C.h>
#include <Wire.h>

#define GPIO_MOT_LEFT_EN 5
#define GPIO_MOT_RIGHT_EN 6
#define GPIO_MOT_LEFT_DIR 4
#define GPIO_MOT_RIGHT_DIR 7
#define GPIO_SENSOR_SERVO 8
#define GPIO_BUZZER 3
#define GPIO_KEY A7
#define GPIO_LED 13

#define NAVI_DISTANCE_K 0.04
#define NAVI_MOT_PULSEWIDTH 100

#define IDLE_L 127
#define IDLE_R 127

#define SCAN_STARTPOS 50
#define SCAN_ENDPOS 130
#define SCAN_CENTER 90

#define CONTROL_KP 3
#define CONTROL_RUNSPEED 60
#define CONTROL_TURNSPEED 50

#define NAVI_TURNTHRE 6

#define SIGNAL_COIN 20000

#define SERVO_SLPTIME 3

#define SCAN_SLPTIME 50

const int CSB = 10;

//Hardware driver 
Servo ScannerArm;
LiquidCrystal_I2C lcd(0x27,16,2);


void SetMotorDuty(byte L,byte R) {
        analogWrite(GPIO_MOT_LEFT_EN,L);
        analogWrite(GPIO_MOT_RIGHT_EN,R);
}

void Motor_Crank() {
        digitalWrite(GPIO_MOT_LEFT_DIR,HIGH);
        digitalWrite(GPIO_MOT_RIGHT_DIR,HIGH);
}

void Motor_Standby() {
        digitalWrite(GPIO_MOT_LEFT_DIR,LOW);
        digitalWrite(GPIO_MOT_RIGHT_DIR,LOW);
}

void GPIO_Init() {
        pinMode(GPIO_MOT_LEFT_EN,OUTPUT);
        pinMode(GPIO_MOT_RIGHT_EN,OUTPUT);
        pinMode(GPIO_MOT_LEFT_DIR,OUTPUT);

        pinMode(GPIO_MOT_RIGHT_DIR,OUTPUT);

        pinMode(GPIO_BUZZER,OUTPUT);
        pinMode(GPIO_LED,OUTPUT);
        SetMotorDuty(0,0);
}


void Serial_LDCSensor_Init() {
        unsigned int data = 0;
  Serial.begin(9600);
  // start SPI library/ activate BUS
  SPI.begin();
 
  pinMode(CSB, OUTPUT); 
 SPI.setBitOrder(MSBFIRST);
  SPI.setDataMode(SPI_MODE0); // CPOL = 0 and CPH = 0 mode 3 also works
  SPI.setClockDivider(SPI_CLOCK_DIV4); // set SCLK @ 4MHz, LDC1000 max is 4MHz DIV2 also works
  
    // set power mode to idle to configure stuff
  digitalWrite(CSB, LOW);
  SPI.transfer(0x0B);
  SPI.transfer(0x00);
  digitalWrite(CSB, HIGH);
  delay(100);

 // Set RpMax
  digitalWrite(CSB, LOW);
  SPI.transfer(0x01);
  SPI.transfer(0x0E);
  digitalWrite(CSB, HIGH);
  delay(100);
  // Set RpMin
  digitalWrite(CSB, LOW);
  SPI.transfer(0x02);
  SPI.transfer(0x3B);
  digitalWrite(CSB, HIGH);
  delay(100);
  
   // Set Sensor frequency
  digitalWrite(CSB, LOW);
  SPI.transfer(0x03);
  SPI.transfer(0x94);
  digitalWrite(CSB, HIGH);
  delay(100);
  
   // Set LDC configurationn
  digitalWrite(CSB, LOW);
  SPI.transfer(0x04);
  SPI.transfer(0x17);
  digitalWrite(CSB, HIGH);
  delay(100);
  
   // Set clock configuration
  digitalWrite(CSB, LOW);
  SPI.transfer(0x05);
  SPI.transfer(0x00);
  digitalWrite(CSB, HIGH);
  delay(100);
  
  
  // disable all interrupt modes
  digitalWrite(CSB, LOW);
  SPI.transfer(0x0A);
  SPI.transfer(0x00);
  digitalWrite(CSB, HIGH);
  // set thresh HiLSB value
  digitalWrite(CSB, LOW);
  SPI.transfer(0x06);
  SPI.transfer(0x50);
  digitalWrite(CSB, HIGH);
  delay(100);
  // set thresh HiMSB value
  digitalWrite(CSB, LOW);
  SPI.transfer(0x07);
  SPI.transfer(0x14);
  digitalWrite(CSB, HIGH);
  delay(100);
  // set thresh LoLSB value
  digitalWrite(CSB, LOW);
  SPI.transfer(0x08);
  SPI.transfer(0xC0);
  digitalWrite(CSB, HIGH);
  delay(100);
  // set thresh LoMSB value
  digitalWrite(CSB, LOW);
  SPI.transfer(0x09);
  SPI.transfer(0x12);
  digitalWrite(CSB, HIGH);
  delay(100);
  
  // set power mode to active mode
  digitalWrite(CSB, LOW);
  SPI.transfer(0x0B);
  SPI.transfer(0x01);
  digitalWrite(CSB, HIGH);
  delay(100);
        
}

void Alarm(byte type) {
        switch(type)
        {
        case 1:
        tone(GPIO_BUZZER,1109,220);
        delay(220);
        tone(GPIO_BUZZER,1245,220);
        delay(220);
        tone(GPIO_BUZZER,1397,220);
        delay(220);
        noTone(GPIO_BUZZER);
        break;
        
        case 2: 
        tone(GPIO_BUZZER,1397,220);
        delay(220);
        tone(GPIO_BUZZER,1109,220);
        delay(220);
        tone(GPIO_BUZZER,1661,220);
        delay(220);
        noTone(GPIO_BUZZER);
        break;
        
        case 3: 
        tone(GPIO_BUZZER,1109,50);
        delay(50);
        noTone(GPIO_BUZZER);
        break;
        
        case 4:
        tone(GPIO_BUZZER,1397,150);
        delay(150);
        tone(GPIO_BUZZER,1661,150);
        delay(150);
        tone(GPIO_BUZZER,2794,150);
        delay(150);
        tone(GPIO_BUZZER,2217,150);
        delay(150);
        tone(GPIO_BUZZER,2489,150);
        delay(150);
        tone(GPIO_BUZZER,3322,150);
        delay(150);
        }
}

void Servo_Init() {
        ScannerArm.attach(GPIO_SENSOR_SERVO);
        ScannerArm.write(90);
}

void Display_Init(){
        lcd.init();
        lcd.backlight();
        //lcd.blink();
}

bool CoinFoundFlag = false;
 
int LDC_SingleScan() {
    unsigned int val = 0;
  unsigned int dataLSB = 0;
  unsigned int dataMSB = 0;
  unsigned int proximitydata = 0;
  
  // Read proximity data LSB register
  digitalWrite(CSB, LOW);
  SPI.transfer(0xA1); // 0x80 + 0x21
  dataLSB = SPI.transfer(0x00);
  digitalWrite(CSB, HIGH);
  delay(SERVO_SLPTIME);
  
  // Read proximity data MSB register
  digitalWrite(CSB, LOW);
  SPI.transfer(0xA2); // 0x80 + 0x22
  dataMSB = SPI.transfer(0x00);
  digitalWrite(CSB, HIGH);
  delay(SERVO_SLPTIME);
  
  proximitydata = ((unsigned int)dataMSB << 8) | (dataLSB);// combine two registers to form 16bit resolution proximity data
  if(proximitydata == 0) Alarm(3);
  Serial.println(proximitydata);
 // Serial.print("\t");
  return proximitydata;
}

byte LDC_AutoScan() {
        static signed char ScanDirection = 1;
        int MaxSignalOnPos = 0;
        int MaxSignal = 0;
        int LDCReadBuf = 0;
        int Start;
        int End;
        if(ScanDirection == 1)
        {
        Start = SCAN_STARTPOS;
        End = SCAN_ENDPOS;
        }
        else 
        {
        End = SCAN_STARTPOS;
        Start = SCAN_ENDPOS;
        }
        for(int CurrentPos = Start ; CurrentPos != End ; CurrentPos += ScanDirection)
        {
                LDCReadBuf = LDC_SingleScan();
                ScannerArm.write(CurrentPos);
                if(LDCReadBuf > MaxSignal)
                {
                        MaxSignal = LDCReadBuf;
                        MaxSignalOnPos = CurrentPos;
                }
        }
        ScanDirection = -ScanDirection;
        if(MaxSignal >= SIGNAL_COIN) {
        Alarm(4);
        CoinFoundFlag = true;
        }
        return MaxSignalOnPos;
}

int LDCValueMax = 1;
int LDCValueMin = 0;

void LDC_StartCalibration()
{        
        
}

void LCD_Format() {
        lcd.setCursor(0,0);
        lcd.print("STATUS  Dir=    ");
        lcd.setCursor(0,1);
        lcd.print("Dst=    Tim=    ");
}

void LCD_ShowInfo(int dir,int dst,int time) {
        lcd.setCursor(12,0);
        lcd.print(dir);
        lcd.setCursor(4,1);
        lcd.print(dst);
        lcd.setCursor(12,1);
        lcd.print(time);
}

byte SpeedWidthLimit(int orig){
        byte dest = orig;
        if(orig >= 255) dest = 254;
        if(orig <= 0 ) dest = 1;
        return dest;
}

int Run_Time;

void setup() {
        Serial_LDCSensor_Init();
        GPIO_Init();
        Servo_Init();
        Display_Init();
        lcd.setCursor(0,0);
        lcd.print("Press any key");
        lcd.setCursor(0,1);
        lcd.print("   [PLL MODE]");
        Alarm(1);
        while(analogRead(GPIO_KEY) > 900);
        Alarm(2);
        delay(1000);
        SetMotorDuty(IDLE_L,IDLE_R);
        delay(1000);
        Run_Time = millis() / 1000;
//        while(1) LDC_SingleScan();
}

int Distance = 0;
void loop() {
        int Duty_Left;
        int Duty_Right;
        int PeakAngle = LDC_AutoScan() - 90;
        if(PeakAngle <= NAVI_TURNTHRE && PeakAngle >= -NAVI_TURNTHRE)
        {
        Duty_Left = IDLE_L + CONTROL_RUNSPEED + (CONTROL_KP * PeakAngle);
        Duty_Right = IDLE_R + CONTROL_RUNSPEED - (CONTROL_KP * PeakAngle);
        }
        else 
        {
        Duty_Left = IDLE_L + CONTROL_TURNSPEED + (CONTROL_KP * PeakAngle);
        Duty_Right = IDLE_R + CONTROL_TURNSPEED - (CONTROL_KP * PeakAngle);
        }
        Motor_Crank();
        if(CoinFoundFlag)
        {
        SetMotorDuty(IDLE_L + CONTROL_RUNSPEED + 20,IDLE_R + CONTROL_RUNSPEED + 20);
        CoinFoundFlag = false;
        }
        else
        {
        SetMotorDuty(SpeedWidthLimit(Duty_Left),SpeedWidthLimit(Duty_Right));
        }
        
        Distance += NAVI_DISTANCE_K * CONTROL_RUNSPEED;
        
        delay(NAVI_MOT_PULSEWIDTH);
        
        SetMotorDuty(IDLE_L,IDLE_R);
        LCD_Format();
        LCD_ShowInfo(PeakAngle , Distance , millis() / 1000 - Run_Time);
        Motor_Standby();
        //Serial.println(LDC_AutoScan());
}


</font>
复制代码