16-01-2017, 12:06
Kod:
//There id more information aboat this code in Page 26 of "Instruction manual-English.pdf"
// This code is for the bluetooth and infrared controlled ultrasonic arduino car.
// By default, the buttons 2, 4, 6 en 8 of the remote will move the car in infrared mode.
// Ultrasonic mode is enabled by the play/pause button.
// Track following is enabled by the EQ button.
// Button 5 will stop whatever the car is doing and enable button 2, 4, 6 en 8 again.
// Bluetooth is always enabled and available while in infrared mode. The password is 1234.
//
// Android car control app can be found here:
// https://play.google.com/store/apps/details?id=braulio.calle.bluetoothRCcontroller&hl=en
//Code based on code found at icstation.com and banggood.com (same sources)
//Modifications
// Continuous movement possible with both bluetooth and ir control
// Customization of servo angles
// Moved up all constants that might need customization
// Added simular code in functions, reducing code lines and complexity
// Broken down long functions in smaller ones to reduce complexity
// Removed unused signals
// Reduced global variables by creating local variables
//********Include libraries*********************************************
#include <IRremote.h>
#include <Servo.h>
#include <avr/interrupt.h>
// Defining IR_GLOBAL here allows us to declare the instantiation of global variables
#define IR_GLOBAL
# include "IRremote.h"
# include "IRremoteInt.h"
#undef IR_GLOBAL
//+=============================================================================
// The match functions were (apparently) originally MACROs to improve code speed
// (although this would have bloated the code) hence the names being CAPS
// A later release implemented debug output and so they needed to be converted
// to functions.
// I tried to implement a dual-compile mode (DEBUG/non-DEBUG) but for some
// reason, no matter what I did I could not get them to function as macros again.
// I have found a *lot* of bugs in the Arduino compiler over the last few weeks,
// and I am currently assuming that one of these bugs is my problem.
// I may revisit this code at a later date and look at the assembler produced
// in a hope of finding out what is going on, but for now they will remain as
// functions even in non-DEBUG mode
//
//+========================================================
// Due to sensor lag, when received, Marks tend to be 100us too long
//
//+========================================================
// Due to sensor lag, when received, Spaces tend to be 100us too short
//
//+=============================================================================
// Interrupt Service Routine - Fires every 50uS
// TIMER2 interrupt code to collect raw data.
// Widths of alternating SPACE, MARK are recorded in rawbuf.
// Recorded in ticks of 50uS [microseconds, 0.000050 seconds]
// 'rawlen' counts the number of entries recorded so far.
// First entry is the SPACE between transmissions.
// As soon as a the first [SPACE] entry gets long:
// Ready is set; State switches to IDLE; Timing of SPACE continues.
// As soon as first MARK arrives:
// Gap width is recorded; Ready is cleared; New logging starts
//
ISR (TIMER_INTR_NAME)
{
TIMER_RESET;
// Read if IR Receiver -> SPACE [xmt LED off] or a MARK [xmt LED on]
// digitalRead() is very slow. Optimisation is possible, but makes the code unportable
uint8_t irdata = (uint8_t)digitalRead(irparams.recvpin);
irparams.timer++; // One more 50uS tick
if (irparams.rawlen >= RAWBUF) irparams.rcvstate = STATE_OVERFLOW ; // Buffer overflow
switch(irparams.rcvstate) {
//......................................................................
case STATE_IDLE: // In the middle of a gap
if (irdata == MARK) {
if (irparams.timer < GAP_TICKS) { // Not big enough to be a gap.
irparams.timer = 0;
} else {
// Gap just ended; Record duration; Start recording transmission
irparams.overflow = false;
irparams.rawlen = 0;
irparams.rawbuf[irparams.rawlen++] = irparams.timer;
irparams.timer = 0;
irparams.rcvstate = STATE_MARK;
}
}
break;
//......................................................................
case STATE_MARK: // Timing Mark
if (irdata == SPACE) { // Mark ended; Record time
irparams.rawbuf[irparams.rawlen++] = irparams.timer;
irparams.timer = 0;
irparams.rcvstate = STATE_SPACE;
}
break;
//......................................................................
case STATE_SPACE: // Timing Space
if (irdata == MARK) { // Space just ended; Record time
irparams.rawbuf[irparams.rawlen++] = irparams.timer;
irparams.timer = 0;
irparams.rcvstate = STATE_MARK;
} else if (irparams.timer > GAP_TICKS) { // Space
// A long Space, indicates gap between codes
// Flag the current code as ready for processing
// Switch to STOP
// Don't reset timer; keep counting Space width
irparams.rcvstate = STATE_STOP;
}
break;
//......................................................................
case STATE_STOP: // Waiting; Measuring Gap
if (irdata == MARK) irparams.timer = 0 ; // Reset gap timer
break;
//......................................................................
case STATE_OVERFLOW: // Flag up a read overflow; Stop the State Machine
irparams.overflow = true;
irparams.rcvstate = STATE_STOP;
break;
}
// If requested, flash LED while receiving IR data
if (irparams.blinkflag) {
if (irdata == MARK)
if (irparams.blinkpin) digitalWrite(irparams.blinkpin, HIGH); // Turn user defined pin LED on
else BLINKLED_ON() ; // if no user defined LED pin, turn default LED pin for the hardware on
else if (irparams.blinkpin) digitalWrite(irparams.blinkpin, LOW); // Turn user defined pin LED on
else BLINKLED_OFF() ; // if no user defined LED pin, turn default LED pin for the hardware on
}
}
//Pin assignments and global variables per function. Customize if needed
//*******Pin assignments Motor board and IR receiver********************
const int MotorRight1 = 7;
const int MotorRight2 = 8;
const int MotorLeft1 = 9;
const int MotorLeft2 = 10;
const int MotorRightPWM = 6;
const int MotorLeftPWM = 11;
const int irReceiverPin = A0;
const int servoPin = 2; //orange wire
int iSpeed = 255; //speed, range 0 to 255
const int LedPin=13;
//******Infrared key bindings********************************************
const long IRfront = 0x00FF18E7; //go straight: button 2
const long IRback = 0x00FF4AB5; //go back : button 8
const long IRturnright = 0x00FF10EF; //turn right : button 6
const long IRturnleft = 0x00FF5AA5; //turn left : button 4
const long IRstop = 0x00FF38C7; //stop : button 5
const long IRcny70 = 0x00FFE01F; //CNY70 automatic mode: button EQ
const long IRAutorun = 0x00FF22DD; //Ultrasonic mode : button play/pause
//******Track following pin assignments and signals**********************
const int SensorLeft = 5; //
const int SensorMiddle = 4 ; //
const int SensorRight = 3; //
IRrecv irrecv(irReceiverPin); // IRrecv signal
decode_results infrared; // decode result
//*******Ultrasonic pin assignments and signals**************************
const int echoPin = 12; // ultrasonic receive=echo pin
const int triggerPin = 13; // ultrasonic send=trigger pin
Servo myservo; // define myservo
const int degreesForward = 130; //nr degrees to look forward
const int degreesLeft = 60; //nr degrees to look left
const int degreesRight = 180; //nr degrees to look right
const int delay_time = 250; // servo motor delay
const int Fgo = 8; // go straight
const int Rgo = 6; // turn right
const int Lgo = 4; // turn left
const int Bgo = 2; // go back
//*****Bluetooth signals**************************************************
char val; //stores received character. Needs to be global to perform continuous movement
//*********General SETUP: activate pins***********************************
void setup() {
//start receiving serial infor
Serial.begin(9600);
//motor connections
pinMode(MotorRight1, OUTPUT); //
pinMode(MotorRight2, OUTPUT); //
pinMode(MotorLeft1, OUTPUT); //
pinMode(MotorLeft2, OUTPUT); //
pinMode(MotorRightPWM, OUTPUT); //enable for right side motor
pinMode(MotorLeftPWM, OUTPUT); //enable for right side motor
irrecv.enableIRIn(); // start infrared decode
myservo.write(degreesForward); // will make head look in front
//black track following
pinMode(SensorLeft, INPUT);
pinMode(SensorMiddle, INPUT);
pinMode(SensorRight, INPUT);
//Ultra sonic
//digitalWrite(2,HIGH); //what is this pin for?
pinMode(echoPin, INPUT);
pinMode(triggerPin, OUTPUT);
myservo.attach(servoPin);
}
//**************Movement functions******************************
void advance(int d) { //go straight
digitalWrite(MotorRight1, HIGH);
digitalWrite(MotorRight2, LOW);
digitalWrite(MotorLeft1, HIGH);
digitalWrite(MotorLeft2, LOW);
analogWrite(MotorRightPWM, iSpeed);
analogWrite(MotorLeftPWM, iSpeed);
delay(d * 10);
}
void right(int d) { //turn right (single wheel)
digitalWrite(MotorLeft1, LOW);
digitalWrite(MotorLeft2, HIGH);
digitalWrite(MotorRight1, LOW);
digitalWrite(MotorRight2, LOW);
analogWrite(MotorRightPWM, iSpeed);
analogWrite(MotorLeftPWM, iSpeed);
delay(d * 10);
}
void left(int d) {//turn left(single wheel)
digitalWrite(MotorRight1, LOW);
digitalWrite(MotorRight2, HIGH);
digitalWrite(MotorLeft1, LOW);
digitalWrite(MotorLeft2, LOW);
analogWrite(MotorRightPWM, iSpeed);
analogWrite(MotorLeftPWM, iSpeed);
delay(d * 10);
}
void turnR(int d) {//turn right (two wheels)
digitalWrite(MotorRight1, HIGH);
digitalWrite(MotorRight2, LOW);
digitalWrite(MotorLeft1, LOW);
digitalWrite(MotorLeft2, HIGH);
analogWrite(MotorRightPWM, iSpeed);
analogWrite(MotorLeftPWM, iSpeed);
delay(d * 10);
}
void turnL(int d) {//turn left (two wheels)
digitalWrite(MotorRight1, LOW);
digitalWrite(MotorRight2, HIGH);
digitalWrite(MotorLeft1, HIGH);
digitalWrite(MotorLeft2, LOW);
analogWrite(MotorRightPWM, iSpeed);
analogWrite(MotorLeftPWM, iSpeed);
delay(d * 10);
}
void stopp(int d) { //stop
digitalWrite(MotorRight1, LOW);
digitalWrite(MotorRight2, LOW);
digitalWrite(MotorLeft1, LOW);
digitalWrite(MotorLeft2, LOW);
analogWrite(MotorRightPWM, iSpeed);
analogWrite(MotorLeftPWM, iSpeed);
delay(d * 10);
}
void back(int d) { //go back
digitalWrite(MotorRight1, LOW);
digitalWrite(MotorRight2, HIGH);
digitalWrite(MotorLeft1, LOW);
digitalWrite(MotorLeft2, HIGH);
analogWrite(MotorRightPWM, iSpeed);
analogWrite(MotorLeftPWM, iSpeed);
delay(d * 10);
}
//************Ultrasonic distance calculator*************************************
//detect distance for given angles and print char + direction
float getDistance(int degrees, char dir) {
myservo.write(degrees);
digitalWrite(triggerPin, LOW); // ultrasonic echo low level in 2us
delayMicroseconds(2);
digitalWrite(triggerPin, HIGH); // ultrasonic echo high level in 10us, at least 10us
delayMicroseconds(10);
digitalWrite(triggerPin, LOW); // ultgrasonic echo low level
float distance = pulseIn(echoPin, HIGH); // read time
distance = distance / 5.8 / 10; // turn time to distance
Serial.print(dir); //
Serial.print(" distance: "); //
Serial.print(int(distance)); // output distance (mm)
Serial.print("\n");
return distance;
}
//*************Ultrasonic direction decision making******************************
//measurements three angles (front, left, right
int getDirectionFromdetection() {
int Fspeedd = 0; // front distance
int Rspeedd = 0; // right distance
int Lspeedd = 0; // left distance
int delay_time = 250; //
int directionn =0;
//get front distance
Fspeedd = getDistance(degreesForward, 'F');
// if distance is less than 10mm
if (Fspeedd < 10) {
stopp(1); // clear output
directionn = Bgo;
}
// if distance less than 25 mm
else if (Fspeedd < 25) {
stopp(1); // clear output
Lspeedd = getDistance(degreesLeft, 'L'); // detection distance on left side
delay(delay_time); // waiting for the servo motor to become stable
Rspeedd = getDistance(degreesRight, 'R'); // detection distance on right side
delay(delay_time); // waiting for servo motor to be stable
// if left distance greater than right
if (Lspeedd > Rspeedd) {
directionn = Lgo; // go left
}
if (Lspeedd <= Rspeedd) {//if left distance less than right
directionn = Rgo; //go right
}
if (Lspeedd < 15 && Rspeedd < 15) { //if distance less 10mm both right and left
directionn = Bgo; //go back
}
}
else {
directionn = Fgo; //go straight
}
return directionn;
}
void autoRunUsingUltraSonic() {
bool stopPressed;
int directionn = 0; // front=8, back=2, left=4, right=6
while (IRAutorun) {
myservo.write(80); // make the servo motor reset
directionn = getDirectionFromdetection();
stopPressed = stopCommandPressed();
if (stopPressed) {
break;
}
else if (directionn == Fgo) { //go straight
infrared.value = 0;
advance(5); //
Serial.print(" Advance "); //
Serial.print(" ");
}
else if (directionn == Bgo) { //go back
infrared.value = 0;
back(8); //
turnL(3); //
Serial.print(" Reverse "); //
}
else if (directionn == Rgo) { //turn right
infrared.value = 0;
back(1);
turnR(60); //
Serial.print(" Right "); //
}
else if (directionn == Lgo) { //turn left
infrared.value = 0;
back(1);
turnL(60);
Serial.print(" Left ");
}
}
infrared.value = 0;
}
//*************************Bluetooth functionality***********************
//Bluetooth commands
void bluetoothCommand() {
if (Serial.available()) { //check if bluetooth command available
val = Serial.read();
Serial.write(val);
}
if (val == 'F') { // Forward
advance(10);
}
else if (val == 'S') { // Stop Forward
stopp(10) ;
val = Serial.read(); //read value again, otherwise can't continu with infrared
}
else if (val == 'B') { // Backwards
back(10);
}
else if (val == 'R') { // Right
turnL(10);
}
else if (val == 'L') { // Left
turnR(10);
}
else if (val == 's') { // Stop, not used though
stopp(10 ) ;
}
else if (int(val) >= 49 && int(val) <= 57) { //set speed
iSpeed = (int(val)-48)*26;
Serial.println("Speed set to: " + iSpeed);
}
else if (val == 'q') { //set speed
iSpeed = 255;
digitalWrite(LedPin,HIGH);
Serial.println("Speed set to: " + iSpeed);
}
else if (val == 'W') {
digitalWrite(LedPin,HIGH);
}
else if (val == 'w') {
digitalWrite(LedPin,LOW);
}
}
//Check if stop command on remote is pressed (button 5)
bool stopCommandPressed(){
bool stopPressed = false;
if (irrecv.decode(&infrared)) {
irrecv.resume();
Serial.println(infrared.value, HEX);
if (infrared.value == IRstop) {
stopp(10);
stopPressed = true;
}
}
return stopPressed;
}
void followBlackLine() {
bool stopPressed;
int SL; //sensor left
int SM; //sensor middle
int SR; //sensor right
while (IRcny70) {
SL = digitalRead(SensorLeft);
SM = digitalRead(SensorMiddle);
SR = digitalRead(SensorRight);
if (SM == HIGH) {//middle sensor in black area
if (SL == LOW & SR == HIGH) {//left sensor in black area, right sensor in white, turn left
left(50);
}
else if (SR == LOW & SL == HIGH) {//left white, right black, run right
right(50);
}
else { // left and right both in white, go straight
advance(50);
}
}
// middle sensor in white area
else {
if (SL == LOW & SR == HIGH) { // left black ,right white, turn left
left(50);
}
else if (SR == LOW & SL == HIGH) {
right(50);
}
else { //left and right both in white, stop
stopp(50);
}
}
stopPressed = stopCommandPressed();
if (stopPressed) {
break;
}
}
infrared.value = 0;
}
//**************************************MAIN LOOP***************************************
void loop() {
//bluetooth commands
bluetoothCommand();
//************************************normal remote control mode ********
// decoding success 'receive infrared signal
if (irrecv.decode(&infrared)) {
if (infrared.value == IRfront) {
advance(0); //go straigt
}
else if (infrared.value == IRback) {
back(0); //go back
}
else if (infrared.value == IRturnright) {
turnR(0); // go right
}
else if (infrared.value == IRturnleft) {
turnL(0); // go left
}
else if (infrared.value == IRstop) {//stop
stopp(0);
}
//********************follow black line********cny70 automatic mode********
else if (infrared.value == IRcny70) {
followBlackLine();
}
//***********************ultrasonic automatic mode***************************
else if (infrared.value == IRAutorun ) {
autoRunUsingUltraSonic();
myservo.write(degreesForward); // will make head look in front
}
//********************wait a little before continuing**************************
irrecv.resume();
delay(300);
}
else {
stopp(0);
}
}Przygode z Arduino zacząłem dość niedawno od pojazdu o dwóch funkcjonalnościach tj. sterowanie poprzez Bletooth oraz tryb autonomiczny poprzez czujnik ultradźwiękowy, zdjęcie pojazdu w załączniku.
Może obrałem złą drogę ale dorwałem kod który obsługuje pojazd pod kątem irdy, line trackera, modułu bluetooth oraz wspomnianego czujnika ultradźwiekowego. Kod dla mnie jako osoby początkującej jest nie do końca zrozumiały.
Podjąłby sie ktoś próby przerobienia go aby został tylko moduł Bluetooth oraz tryb autonomiczny oparty na czujniku ultradziekowym ? Pomogłoby mi to stawiać pierwsze kroki oraz stopniowo modyfikować pojazd.
Z góry dziękuje za pomoc
ps. kod zamieszczam w załączniku