走钢丝+扫雷的巡迹小车 DF创客社区

virtualwiz 发表于 2016-9-20 20:38:49

走钢丝+扫雷的巡迹小车

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

巡迹车估计是入门Arduino的同学们玩厌了的项目了吧{:5_173:}

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

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

{:5_172:}
static/image/hrline/4.gif

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

http://player.youku.com/player.php/sid/XMTczMjA0NjQ0MA==/v.swf

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

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

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

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

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

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

static/image/hrline/2.gif

https://mc.dfrobot.com.cn/forum.php?mod=image&aid=30117&size=300x300&key=c2ec3a124adf1a06&nocache=yes&type=fixnone

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

https://wiki.dfrobot.com.cn/images/8/87/Romeov11xxx.png

送上代码~~据说写代码要规范，别人看到才会舒服。{:5_169:}
<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("STATUSDir= ");
   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(" ");
   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>

Rockets 发表于 2016-9-21 09:47:33

关于代码规范啊，我觉得做的还是不错的嘛，哈哈。缩进后看起来很舒服。
关于巡线，其实真的可以学习很多东西的。

Ash 发表于 2016-9-21 16:59:25

V神代码好工整！！

dsweiliang 发表于 2016-9-22 14:15:13

感谢分享

iooops 发表于 2016-10-1 20:24:01

只见到一长串的Arduino代码

svw 发表于 2016-10-21 19:21:46

V神值得学习，还是那个问题，电源用啥尼？

swanglei 发表于 2016-11-8 14:35:46

看视频更有意思！想法挺不错的。。。脑洞很大

飞猪的小号 发表于 2016-12-4 22:08:59

16年湖北大学生电子设计竞赛的题目。我们仪器组做的是电子秤，用的是28335，控制组做的就是这个题

DDIsKII 发表于 2017-3-6 22:35:21

第一次看见如此画风清奇的寻线方式····感觉打开了新世界的大门。。。

页: [1]

DF创客社区 - 分享创造的喜悦's Archiver

走钢丝+扫雷的巡迹小车