mBot Ranger 高速循線範例＿
grUwwSxMCJw#include <Arduino.h>
#include <MeAuriga.h>
#include "MeLineFollowerArray.h"
MeEncoderOnBoard Encoder_1(SLOT1);
MeEncoderOnBoard Encoder_2(SLOT2);
MeLightSensor lightsensor_1(12);
MeLightSensor lightsensor_2(11);
MeBuzzer buzzer;
MeLineFollowerArray linefollower(PORT_6);
#define Error1 1
#define Error2 2
#define Error3 3
#define Kp 15
#define Ki 0.15
#define Kd 0.03
uint8_t sData;
uint8_t D1;
uint8_t D2;
uint8_t D3;
uint8_t D4;
uint8_t D5;
uint8_t D6;
float previous_error = 0;
float integral = 0;
float derivative = 0;
intSpeed= 160;
float output;
byte Left;
void setup()
{
//Set PWM 8KHz
TCCR1A = _BV(WGM10);
TCCR1B = _BV(CS11) | _BV(WGM12);
TCCR2A = _BV(WGM21) | _BV(WGM20);
TCCR2B = _BV(CS21);
Serial.begin(9600);
buzzer.setpin(45);
}
void loop()
{
while(!((lightsensor_1.read()) < (10)));
buzzer.tone(1047, 500);
delay(500);
do
{
int Err = getErr();
if(D1 == 1)
{
Left = 1;
}
if(D6 == 1)
{
Left = 0;
}
if(Err == 99)
{
if(Left == 1)
{
Speed -= 5;
moto(0,Speed);
do
{
Err = getErr();
}while((D1+D6) == 0);
}
else
{
Speed -= 5;
moto(Speed,0);
do
{
Err = getErr();
}while((D1+D6) == 0);
}
}
else
{
if((Speed < 160) & (Err < 2)) Speed+=1;
if((Speed > 100) & (Err > 2)) Speed-=2;
integral = integral + Err;
derivative = Err - previous_error;
output = Kp*Err + Ki*integral + Kd*derivative;
moto(int(Speed-output),int(Speed+output));
previous_error = Err;
}
}while(!((lightsensor_2.read()) < (10)));
moto(0,0);
delay(500);
buzzer.tone(262, 500);
}
int getErr()
{
sData = linefollower.getValue();
D1 = ~(sData>>0)&1;
D2 = ~(sData>>1)&1;
D3 = ~(sData>>2)&1;
D4 = ~(sData>>3)&1;
D5 = ~(sData>>4)&1;
D6 = ~(sData>>5)&1;
int downD = D1+D2+D3+D4+D5+D6;
int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
if(downD == 0)
{
return 99;
}
else
{
return upD/downD;
}
}
void moto(int Speed_L,int Speed_R)
{
Encoder_2.setMotorPwm(Speed_L);
Encoder_1.setMotorPwm(-Speed_R);
}
MeLineFollowerArray.cpp
#include "MeLineFollowerArray.h"
#ifdef ME_PORT_DEFINED
MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
{
}
MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
{
_DataPin = mePort.s2;
pinMode(_DataPin, OUTPUT);
digitalWrite(_DataPin, HIGH);
}
#else // ME_PORT_DEFINED
MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
{
_DataPin = pin;
pinMode(_DataPin, OUTPUT);
digitalWrite(_DataPin, HIGH);
}
#endif // ME_PORT_DEFINED
void MeLineFollowerArray::setpin(uint8_t pin)
{
_DataPin = pin;
pinMode(_DataPin, OUTPUT);
digitalWrite(_DataPin, HIGH);
#ifdef ME_PORT_DEFINED
s2 = pin;
#endif
}
uint8_t MeLineFollowerArray::getValue()
{
uint32_t LOW_level_read_time;
uint32_t HIGH_level_read_time;
uint32_t time_out_flag;
uint8_t Sensor_Data;
static uint8_t old_data = 0xff;
pinMode(_DataPin, OUTPUT);
digitalWrite(_DataPin, LOW);
delayMicroseconds(980);
digitalWrite(_DataPin, HIGH);
pinMode(_DataPin, INPUT_PULLUP);
delayMicroseconds(10);
time_out_flag = millis();
while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
LOW_level_read_time = micros();
if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
{
return 0xff;
}
while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
HIGH_level_read_time = micros();
LOW_level_read_time= micros() - LOW_level_read_time; //read 100us LOW level
if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
{
return 0xff;
}
if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
{
return 0xff;
}
while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
LOW_level_read_time= micros();
HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
{
return 0xff;
}
if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
{
return 0xff;
}
for(uint8_t k=0; k<3; k++)
{
Sensor_Data = 0x00;
for(uint8_t i=0;i<8;i++)
{
while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
HIGH_level_read_time = micros();
LOW_level_read_time= micros() - LOW_level_read_time;
if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
{
return 0xff;
}
while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
LOW_level_read_time= micros();
HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
{
Sensor_Data |= (0x80 >> i);
}
else if(HIGH_level_read_time >= 100)
{
return 0xff;
}
if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
{
return 0xff;
}
}
}
while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
HIGH_level_read_time = micros();
LOW_level_read_time= micros() - LOW_level_read_time;
if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
{
return 0xff;
}
pinMode(_DataPin, OUTPUT);
digitalWrite(_DataPin, HIGH);
if( (Sensor_Data == (uint8_t)(Sensor_Data - Sensor_Data)) && (Sensor_Data == (uint8_t)(~(uint8_t)Sensor_Data)))
{
old_data = Sensor_Data;
return Sensor_Data;
}
else
{
return old_data;
}
}
MeLineFollowerArray.h
強力推廣中 哇...要100 個銅錢 daven 發表於 2019-3-6 00:09
哇...要100 個銅錢
認真的回饋本站一些內容, 很容易達成的! 感謝分享，還不夠權限下載 感謝分享Thanks:D 能否用到mblock 5 上面呢？ roboger 發表於 2020-11-13 15:43
能否用到mblock 5 上面呢？
MeLineFollowerArray 在 mBlock5 中, 只有支援 megaPi 控制板, Ranger 無法使用, 如有需求, 我再寫一個擴展上架吧! 无法添加到扩展呀 :'( 高手就是高手
頁:
[1]