Skip to content

Carbot/370 Redux – Now With Added Computers

March 12, 2019

This is largely a cheat – all of the navigation code is running in the arduino. The Raspi is just telling it when to start and stop based on commands sent from my windows machine over ssh and relaying telemetry from the sensors. There’s not a whiff of 370 code in the mix and hardly any pi. The vision is to have the arduino doing the close in work while the 370/raspi sets broad parameters like speed and object avoidance patterns and maybe looks for stall situations.

The current code is trying to keep within a range of distances to the right hand wall and makes a hard left when the clearance in front is too small.

The robot car platform was a christmas gift. It had an ultrasonic sensor that i’m using for the wall distance and i’ve connected the sharp infrared sensor as the forward distance measurement. There are four DC motors on the wheels controlled by a custom motor shield on an arduino clone. It also has a line following sensor, a bluetooth add-on, and an infrared remote control. Either the bluetooth or infrared remotes could do what the Pi is doing but I need the pi in the loop for the telemetry and future considerations. The kit is the Elegoo Smart Robot Car Kit V3.0 and it has a ton of function built in.

#!/usr/bin/python
import sys, time
#import difflib
import pigpio
import termios, fcntl, sys, os

def get_char_keyboard_nonblock():
  fd = sys.stdin.fileno()

  oldterm = termios.tcgetattr(fd)
  newattr = termios.tcgetattr(fd)
  newattr[3] = newattr[3] & ~termios.ICANON & ~termios.ECHO
  termios.tcsetattr(fd, termios.TCSANOW, newattr)

  oldflags = fcntl.fcntl(fd, fcntl.F_GETFL)
  fcntl.fcntl(fd, fcntl.F_SETFL, oldflags | os.O_NONBLOCK)

  c = None

  try:
    c = sys.stdin.read(1)
  except IOError: pass

  termios.tcsetattr(fd, termios.TCSAFLUSH, oldterm)
  fcntl.fcntl(fd, fcntl.F_SETFL, oldflags)

  return c

RX=25 #input 4 of the explorer phat
GO=6  #output 1 of the explorer phat
print "trying bb_serial"
#from https://www.rs-online.com/designspark/raspberry-pi-2nd-uart-a-k-a-bit-banging-a-k-a-software-serial
try:
        pi = pigpio.pi()
        pi.set_mode(RX, pigpio.INPUT)
        pi.bb_serial_read_open(RX, 9600, 8)
        pi.set_mode(6, pigpio.OUTPUT) # GPIO 6 is output 1 of explorer phat
        print "Car Controller:"
        pi.write(6,0)
        ch=get_char_keyboard_nonblock() #does not wait for input - returns a char or none
        while ch!='q':
                if ch=='f': #forward
                    pi.write(6,0)
                    #print hex(ord(ch)),1,
                elif ch=='h': #halt
                    pi.write(6,1)
                    #print hex(ord(ch)),0,
                (count, data) = pi.bb_serial_read(RX)
                if count:
                        sys.stdout.write(data)
                        #for character in data:
                        #  print chr(character), #hex(character)
                ch=get_char_keyboard_nonblock()
        pi.write(6,1)
finally:
        pi.write(6,1)
        print "done"
        pi.bb_serial_read_close(RX)
        pi.stop()
//www.elegoo.com
#include 
#include   //servo library
Servo myservo;      // create servo object to control servo

#define ENB 5   // Left  wheel speed
#define IN1 7   // Left  wheel forward
#define IN2 8   // Left  wheel reverse
#define IN3 9   // Right wheel reverse
#define IN4 11  // Right wheel forward
#define ENA 6   // Right wheel speed
#define carSpeed 220  // initial speed of car >=0 to oldwdist) {//but going the right way
        analogWrite(pwmleft,basespeed);analogWrite(pwmright,basespeed); //proceed
        Serial<<" wf-1\n";
    }
    else{
        analogWrite(pwmleft,lowspeed);analogWrite(pwmright,basespeed); //bear left
        Serial<<" wvlft\n";
    }
}
void toofar(){//if we are further than minwdist from the wall on our right side
    if (wdist<oldwdist) {//but going the right way
        analogWrite(pwmleft,basespeed);analogWrite(pwmright,basespeed); //proceed
        Serial<<" wf-2\n";
    }
    else{
        analogWrite(pwmleft,basespeed);analogWrite(pwmright,lowspeed); //bear right
        Serial<<" wvrt\n";
    }
}

void inthezone(){ //if we are between minwdist and maxwdist
    analogWrite(pwmleft,basespeed);analogWrite(pwmright,basespeed); //proceed
    Serial<<" wf-3\n";
}

void hardleft(){
  analogWrite(ENA, carSpeed);
  analogWrite(ENB, carSpeed);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
  Serial.println(" HL ");
}

//Ultrasonic distance measurement Sub function
int getDistance() { //returns distance in cm
    digitalWrite(Trig, LOW);
    delayMicroseconds(2);
    digitalWrite(Trig, HIGH);
    delayMicroseconds(10);
    digitalWrite(Trig, LOW);
    return (int)pulseIn(Echo, HIGH) / 58;
}
void cruise(){
  if (0!=wdist) oldwdist=wdist; //track the wall distance after the first time
  wdist=getDistance(); //get the wall distance in cm
  Serial<<"cw "<<wdist<<" ";
   if (wdistmaxwdist){
      toofar();
  }else{
      inthezone();
  }
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
  delay(60);
}
int sharpy(){
  int aval=analogRead(0);
  if (aval<85) aval=85;
  int dist=(6787/(aval-3))-4;
   return dist;
}
void loop() {
    if (digitalRead(goPin)){
        fdist=sharpy();
        Serial<<millis()<<" f="<<fdist<minfdist){
           cruise();
       }else{
           hardleft(); //begin left turn
           Serial<<" L\n";
           while(fdist<=minfdist){ //until we're clearing the wall
              delay(1);
              fdist=sharpy();
           }
           Serial<<"K f="<<fdist<<"\n";
           stop();
         }
    }else{
      stop();
    }
 }

https://pinout.xyz/pinout/explorer_phat

https://www.raspberrypi.org/forums/viewtopic.php?t=106410

Advertisements

From → carbot

Leave a Comment

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out /  Change )

Google photo

You are commenting using your Google account. Log Out /  Change )

Twitter picture

You are commenting using your Twitter account. Log Out /  Change )

Facebook photo

You are commenting using your Facebook account. Log Out /  Change )

Connecting to %s

%d bloggers like this: