Welcome to Hackster!
Hackster is a community dedicated to learning hardware, from beginner to pro. Join us, it's free!
Dima Panasenko
Published © GPL3+

4 wheels RC toy with object recognition model on board

Intro into robotics, neural web and ML with arduino, raspberry pi4

IntermediateFull instructions provided869
4 wheels RC toy with object recognition model on board

Things used in this project

Hardware components

Raspberry Pi 4 Model B
Raspberry Pi 4 Model B
×1
Arduino Mega 2560
Arduino Mega 2560
×1
Dual H-Bridge motor drivers L298
SparkFun Dual H-Bridge motor drivers L298
×1
SG90 Micro-servo motor
SG90 Micro-servo motor
×2
Webcam, Logitech® HD Pro
Webcam, Logitech® HD Pro
×1
nRF24 Module (Generic)
×2
SparkFun Logic Level Converter - Bi-Directional
SparkFun Logic Level Converter - Bi-Directional
×1

Hand tools and fabrication machines

Soldering kit
Multimeter

Story

Read more

Code

RPI4 object detection script

Python
just launch it from comand prompt with a path of your model
#!/usr/bin/env python

#import device_patches       # Device specific patches for Jetson Nano (needs to be before importing cv2)

import cv2
import os
import sys, getopt
import signal
import time
from edge_impulse_linux.image import ImageImpulseRunner
import struct
import serial
from gpiozero import Servo
from time import sleep
import math
from gpiozero.pins.pigpio import PiGPIOFactory
factory = PiGPIOFactory()

#-----------servo initialisation----------------------- 
servo_x = Servo(18, 
                    min_pulse_width = 0.0005, 
                    max_pulse_width = 0.0025,
                    pin_factory = factory
                    )

servo_y = Servo(23, 
                    min_pulse_width = 0.0005, 
                    max_pulse_width = 0.0025,
                    pin_factory = factory
                    )

#------------ asign servo func --------------------
def servos(x,y):
   
    if x/2 and y/2 >= 48:
        servo_x.value = -0.25*x/48
        servo_y.value = -0.25*y/48
    else:
        servo_x.value = -1*(0.25*x/48 - 0.25)
        servo_y.value = -1*(0.25*y/48 - 0.25)
    


runner = None
# if you don't want to see a camera preview, set this to False
show_camera = True
if (sys.platform == 'linux' and not os.environ.get('DISPLAY')):
    show_camera = False

def now():
    return round(time.time() * 1000)

def get_webcams():
    port_ids = []
    for port in range(5):
        print("Looking for a camera in port %s:" %port)
        camera = cv2.VideoCapture(port)
        if camera.isOpened():
            ret = camera.read()[0]
            if ret:
                backendName =camera.getBackendName()
                w = camera.get(3)
                h = camera.get(4)
                print("Camera %s (%s x %s) found in port %s " %(backendName,h,w, port))
                port_ids.append(port)
            camera.release()
    return port_ids

def sigint_handler(sig, frame):
    print('Interrupted')
    if (runner):
        runner.stop()
    sys.exit(0)

signal.signal(signal.SIGINT, sigint_handler)

def help():
    print('python classify.py <path_to_model.eim> <Camera port ID, only required when more than 1 camera is present>')

def main(argv):
    try:
        opts, args = getopt.getopt(argv, "h", ["--help"])
    except getopt.GetoptError:
        help()
        sys.exit(2)

    for opt, arg in opts:
        if opt in ('-h', '--help'):
            help()
            sys.exit()

    if len(args) == 0:
        help()
        sys.exit(2)

    model = args[0]

    dir_path = os.path.dirname(os.path.realpath(__file__))
    modelfile = os.path.join(dir_path, model)

    print('MODEL: ' + modelfile)

    with ImageImpulseRunner(modelfile) as runner:
        try:
            model_info = runner.init()
            # model_info = runner.init(debug=True) # to get debug print out
            print('Loaded runner for "' + model_info['project']['owner'] + ' / ' + model_info['project']['name'] + '"')
            labels = model_info['model_parameters']['labels']
            if len(args)>= 2:
                videoCaptureDeviceId = int(args[1])
            else:
                port_ids = get_webcams()
                if len(port_ids) == 0:
                    raise Exception('Cannot find any webcams')
                if len(args)<= 1 and len(port_ids)> 1:
                    raise Exception("Multiple cameras found. Add the camera port ID as a second argument to use to this script")
                videoCaptureDeviceId = int(port_ids[0])
            serial_communication = serial.Serial('/dev/ttyAMA0', 9600) #/dev/ttyAMA0 /dev/ttyS0
            camera = cv2.VideoCapture(videoCaptureDeviceId)
            ret = camera.read()[0]
            if ret:
                backendName = camera.getBackendName()
                w = camera.get(3)
                h = camera.get(4)
                print("Camera %s (%s x %s) in port %s selected." %(backendName,h,w, videoCaptureDeviceId))
                camera.release()
            else:
                raise Exception("Couldn't initialize selected camera.")

            next_frame = 0 # limit to ~10 fps here

            for res, img in runner.classifier(videoCaptureDeviceId):
                if (next_frame > now()):
                    time.sleep((next_frame - now()) / 1000)

                #print('classification runner response', res)
                #print('classification runner response', img)
                if "classification" in res["result"].keys():
                    print('Result (%d ms.) ' % (res['timing']['dsp'] + res['timing']['classification']), end='')
                    for label in labels:
                        score = res['result']['classification'][label]
                        print('%s: %.2f\t' % (label, score), end='')
                    print('', flush=True)
                elif "bounding_boxes" in res["result"].keys():
                    print('Found %d bounding boxes (%d ms.)' % (len(res["result"]["bounding_boxes"]), res['timing']['dsp'] + res['timing']['classification']))
                    for bb in res["result"]["bounding_boxes"]:
                        print('\t%s (%.2f): x=%d y=%d w=%d h=%d' % (bb['label'], bb['value'], bb['x'], bb['y'], bb['width'], bb['height']))
                        img = cv2.rectangle(img, (bb['x'], bb['y']), (bb['x'] + bb['width'], bb['y'] + bb['height']), (255, 0, 0), 1)
                #----------------------------- coordinates yield-----------------------------
                
                data = res['result']
                bb = data['bounding_boxes']
                if len(bb) != 0 and bb[0]['value']>0.8:
                    
                    x = bb[0]['x']
                    y = bb[0]['y']
                    servos(x, y)
                    
                    serial_communication.write(f"{x*1023/96},{y*1023/96}\n".encode())
                    

                if (show_camera):
                    cv2.imshow('edgeimpulse', cv2.cvtColor(img, cv2.COLOR_RGB2BGR))
                    if cv2.waitKey(1) == ord('q'):
                        break
                        serial_communication.close()
                next_frame = now() + 100
        finally:
            if (runner):
                runner.stop()

if __name__ == "__main__":
   main(sys.argv[1:])

Transmitter

Arduino
Scetch for arduino to transmit data via nRF01 radio module
// Arduino pin numbers

#include "SPI.h"
#include "RF24.h"
#include "nRF24L01.h"

#define CE_PIN 9
#define CSN_PIN 10
#define INTERVAL_MS_TRANSMISSION 250

RF24 radio(CE_PIN, CSN_PIN);

const byte address[6] = "00001";

const int SW_pin = 2; // digital pin connected to switch output
const int X_pin = 3; // analog pin connected to X output
const int Y_pin = 4; // analog pin connected to Y output

int x;
int y;

struct payload {
  int x;
  int y;
};

payload payload;

void setup() {
  pinMode(SW_pin, INPUT);
  digitalWrite(SW_pin, HIGH);
  Serial.begin(9600);
  
  radio.begin();
  
  //Append ACK packet from the receiving radio back to the transmitting radio
  radio.setAutoAck(false); //(true|false)
  radio.setChannel(0x6f);
  radio.setCRCLength(RF24_CRC_8);
  //Set the transmission datarate
  radio.setDataRate(RF24_1MBPS); //(RF24_250KBPS|RF24_1MBPS|RF24_2MBPS)
  //Greater level = more consumption = longer distance
  radio.setPALevel(RF24_PA_MAX); //(RF24_PA_MIN|RF24_PA_LOW|RF24_PA_HIGH|RF24_PA_MAX)
  //Default value is the maximum 32 bytes
  radio.setPayloadSize(sizeof(payload));
  //Act as transmitter
  radio.openWritingPipe(address);
  radio.stopListening();
}

void loop() {
  payload.x = analogRead(X_pin);
  payload.y = analogRead(Y_pin);

  radio.write(&payload, sizeof(payload));

  Serial.print("x:");
  Serial.println(payload.x);

  Serial.print("y:");
  Serial.println(payload.y);

  Serial.println("*---------------------*");

  /*Serial.print("Switch:  ");
  Serial.print(digitalRead(SW_pin));
  Serial.print(" | ");
  Serial.print("X-axis: ");
  Serial.print(analogRead(X_pin));
  Serial.print(" | ");
  Serial.print("Y-axis: ");
  Serial.print(analogRead(Y_pin));
  Serial.println(" | ");*/
  delay(50);
}

Reciever

Arduino
Arduino scetch to recieve data with nRF01 radiomodule
#include "SPI.h"
#include "RF24.h"
#include "nRF24L01.h"
#include <Servo.h>
#define CE_PIN 22
#define CSN_PIN 23

const int X_pin = 0; 
const int Y_pin = 1; 

//--------ultrasonic sensor pins --------
int trigPin = 10; 
int echoPin = 11;

const int engineA = 2;
const int engineB = 7;

const int right_engine_in1 = 3;
const int right_engine_in2 = 4;
const int right_engine_in3 = 5;
const int right_engine_in4 = 6;

const byte address[6] = "00001";

unsigned long previousMillis = 0;
const long time_interval = 3000;  // 2 second
long duration;
int cm;
bool flag = false;

RF24 radio(CE_PIN, CSN_PIN);

Servo myservo;
int x;
int y;
int pos;

unsigned long lastRadioTime = 0;
const unsigned long RADIO_TIMEOUT = 1000;



//rpi_data rpi_package;

void setup() {
  myservo.attach(8);
  
  //--------initialize a radio module -----------------
  radio.begin();
  
  //Append ACK packet from the receiving radio back to the transmitting radio
  radio.setAutoAck(false); //(true|false)
  radio.setChannel(0x6f);
  radio.setCRCLength(RF24_CRC_8);
  //Set the transmission datarate
  radio.setDataRate(RF24_1MBPS); //(RF24_250KBPS|RF24_1MBPS|RF24_2MBPS)
  //Greater level = more consumption = longer distance
  radio.setPALevel(RF24_PA_MIN); //(RF24_PA_MIN|RF24_PA_LOW|RF24_PA_HIGH|RF24_PA_MAX)
  //Default value is the maximum 32 bytes
  radio.setPayloadSize(sizeof(payload));
  //Act as receiver
  radio.openReadingPipe(1, address);
  radio.startListening();

  //--------- engines pins initialisation ----------
  pinMode(engineA, OUTPUT);
  pinMode(right_engine_in1, OUTPUT);
  pinMode(right_engine_in2, OUTPUT);

  pinMode(engineB, OUTPUT);
  pinMode(right_engine_in3, OUTPUT);
  pinMode(right_engine_in4, OUTPUT);

  //digitalWrite(SW_pin, HIGH);
  digitalWrite(right_engine_in1, LOW);
  digitalWrite(right_engine_in2, LOW);
  digitalWrite(right_engine_in3, LOW);
  digitalWrite(right_engine_in4, LOW);

  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  Serial.begin(9600);
  Serial1.begin(9600);
}
void loop() {
  
  
  if (radio.available()) {
      radio.read(&payload, sizeof(payload));
      int *x_ptr = &x;
      int *y_ptr = &y;
      *x_ptr = payload.x;
      *y_ptr = payload.y;
      Serial.println("Received radio");
      Serial.println(radio.available());
      Serial.print("payload.x ");
      Serial.println(payload.x);
      Serial.print("payload.y ");
      Serial.println(payload.y);
      lastRadioTime = millis();
  } 
  // --- Fallback to RPi if radio is lost ---
  if (millis() - lastRadioTime > RADIO_TIMEOUT) {
    delay(50);
    data_from_rpi();
    Serial.println("Switched to RPi autonomy");
  }
  
  //--------convert data into dergees of wheels -----------
  float y_value = (float) y;
  float angle_value = -1*(503-y_value)/1023*180;
  int angle_pos = 90 - round(angle_value);
  //Serial.print(angle_pos);
  int distance = distance_sensor();
  if (distance < 10){
    x = 1023;
    y = 0;

  }

  /*------------ moving strait -----------------*/
  if (x > 522){
    drive = map(x, 522, 1023, 0, 255);
    analogWrite(engineA, drive);
    digitalWrite(right_engine_in1, LOW);
    digitalWrite(right_engine_in2, HIGH);

    analogWrite(engineB, drive);
    digitalWrite(right_engine_in3, HIGH);
    digitalWrite(right_engine_in4, LOW);

  }
  /*---------------moving back-------------------*/
  else if (x < 510){
    drive = map(x, 500, 0, 0, 255);
    analogWrite(engineA, drive);
    digitalWrite(right_engine_in1, HIGH);
    digitalWrite(right_engine_in2, LOW);

    analogWrite(engineB, drive);
    digitalWrite(right_engine_in3, LOW);
    digitalWrite(right_engine_in4, HIGH);
  }
  /*----------------neutral----------------------*/
  else{
    digitalWrite(right_engine_in1, LOW);
    digitalWrite(right_engine_in2, LOW);
    digitalWrite(right_engine_in3, LOW);
    digitalWrite(right_engine_in4, LOW);

  }
   
  myservo.write(angle_pos);
  radio.flush_rx();

  Serial.print("distance from us sensor: ");
  Serial.println(distance);

  delay(50);

}

//-----------retrieve data from RPI ------------
// Python script on rpi4 process incoming webcam data with opencv 
// and pretrained object detection model on Edge Impulse
// sends data into arduino via serial1 

void data_from_rpi (){  
  // if frame data are not available, objects are not detected, 
  // car turns around

  bool *flag_ptr = &flag;
  unsigned long startTime = millis();
  while (!Serial1.available()) {
        
        if (millis() - startTime > 3000) {  // Timeout after 2 seconds
          
          Serial.println("autonomous");
          if (*flag_ptr==true){
                x = 100;
                y = 1024;
                *flag_ptr = false;  
          } else {
              x = 900;
              y = 0;
              *flag_ptr = true;
          }
        return;       
  }
  
  // if serial data are available
  if (Serial1.available()) {
      
      String data = Serial1.readStringUntil('\n');
      float horizontal_axes = data.substring(0, data.indexOf(',')).toFloat();
      float vertical_axes  = data.substring(data.indexOf(',') + 1).toFloat();
      x = 100;//rpi_package.x_axes;
      y = 1023.0 - horizontal_axes;
      
      Serial.print("rpi_package.y_axes: ");
      Serial.println(horizontal_axes);
      Serial.print("Serial1 data: ");
      Serial.print(data);
      Serial.println(Serial1.available());
      
  }

  
}
 // -----------ultrasound distance sensor presettings ----------
int distance_sensor(){

  digitalWrite(trigPin, LOW);
  delayMicroseconds(5);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
 
  duration = pulseIn(echoPin, HIGH);
 
  // Convert the time into a distance
  cm = (duration/2) / 29.1;     // Divide by 29.1 or multiply by 0.0343
  inches = (duration/2) / 74;   // Divide by 74 or multiply by 0.0135
  //Serial.print("cm: ");
  //Serial.println(cm);
  return cm;
}

Credits

Dima Panasenko
4 projects • 0 followers
Contact

Comments

Please log in or sign up to comment.