Build a simple visually-guided toy robot car for under $100 with JeVois

In this tutorial we build a simple visually-guided robot car, using the cheapest possible mechanical components and compensating for their brittleness by using high-framerate, closed-loop vision processing.

As a first application, the toy robot card will detect, track, and follow ArUco markers (small 2D barcodes).

In this tutorial, you will learn:

• How to design and build a simple autonomous vehicle with vision processing
• How to program a simple visually-guided behavior on an Arduino

Theory of operation

The robot car will feature a pan/tilt head onto which the JeVois camera will be mounted. This is so that we can very quickly track a moving target before it escapes out of the field of view of the JeVois camera, even though the car may be quite sluggish to turn and orient towards it.

JeVois will detect an ArUco marker and will communicate its coordinates and size to an Arduino responsible for control. The Arduino implements two PD (proportional, differential) controllers to control the pan-tilt head smoothly so that it tracks the moving target. The Arduino then also implements two PID (proportional, integral, differential) controllers to steer and move the car, so as to 1) maintain a fixed distance from the target and 2) steep to zero out any pan angle on the pan-tilt head (i.e., re-center the pan-tilt head by rotating the whole vehicle).

Bill of materials

Part	Price	Where to buy
JeVois A33 Smart Camera	$49.99	https://www.jevoisinc.com/collections/jevois-hardware/products/jevois-a33-smart-machine-vision-camera?variant=36249051018
Dual motor robot chassis	$8.58	http://www.ebay.com/itm/Smart-Robot-Car-Chassis-Kit-Speed-Encoder-Battery-Box-DIY-Kit-2WD-2-Motor-1-48-/132228276291?hash=item1ec96b6043:g:5x0AAOSwwzhZQlla
Pan/tilt head hardware	$1.16	http://www.ebay.com/itm/PT-Pan-Tilt-Camera-Platform-Anti-Vibration-Camera-Servo-Mount-for-Aircraft-FPV-I-/390961293160?hash=item5b071be768:g:5IgAAOSw4YdYz0ug
2x 9g micro servos	$2.64	http://www.ebay.com/itm/NT-9G-Servo-Mini-Micro-For-Trex-Align-450-Rc-Helicopter-Airplane-Foamy-Plane-/112489494262?epid=25005818319&hash=item1a30e576f6:g:zX4AAOSwAuZX18~p
2600mAh battery bank, 1A output	$7.99	https://www.jevoisinc.com/collections/accessories/products/usb-power-bank-2600mah
Mini + Micro USB splitter cable	$4.99	https://www.jevoisinc.com/collections/accessories/products/mini-usb-micro-usb-splitter-cable-15cm-6-in-long
8GB class 10 Micro SD card	$8.99	https://www.jevoisinc.com/collections/accessories/products/high-speed-8gb-microsd-card-with-jevois-software-pre-loaded
Arduino 32u4 compatible	$6.99	https://www.jevoisinc.com/collections/accessories/products/atmega32u4-16mhz-5v-arduino-compatible-micro-controller
Dual 1.2A motor driver PWM	$1.33	http://www.ebay.com/itm/TB6612FNG-Dual-DC-Stepper-Motor-Drive-Controller-Board-Module-Replace-L298N-/191661587054?epid=719645256&hash=item2c9febba6e:g:1PUAAOSwI3RW-22c
AA batteries, screws, wires	$2.34	misc
Total	$95.00	(Prices include shipping to the USA. Shipping to other countries may vary.)

Assembly

• Assemble the car chassis per the provided instructions

• Assemble the pan/tilt head per the provided instructions
• Use the following Arduino connections:

Arduino pin	Connect to
RX	JeVois micro-serial port white wire
TX	JeVois micro-serial port yellow wire
VCC	JeVois micro-serial port red wire
GND	JeVois micro-serial port black wire
3	Motor driver PWM for left motor
2	Motor driver IN1 for left motor
21	Motor driver IN2 for left motor
5	Motor driver PWM for right motor
20	Motor driver IN1 for right motor
4	Motor driver IN2 for right motor
9	PWM control wire of pan servo
10	PWM control wire of tilt servo

Note

The pins used leave enough free pins to connect an optional additional SPI OLED screen should you want to have one on your robot. Search for, e.g., 0.96" OLED SPI on eBay to find a screen.

Also connect the 6V power (from the 4xAA pack) to the power lines of the pan and tilt servos, and to the motor input voltage of the motor controller board. The Arduino and JeVois will be powered directly from the USB battery bank using the splitter mini + micro USB cable. Finally connect the Arduino GND (from USB battery) to motor GND (from 4xAA batteries).

Your rig should roughly look as follows:

JeVois configuration

For our initial testing of this robot we will use the ArUco module with no USB output. Just edit JEVOIS:/config/initscript.cfg as follows:

setmapping2 YUYV 320 240 50.0 JeVois DemoArUco
setpar serlog None
setpar serout Hard
setpar serstyle Normal
streamon

Arduino code

We will use the Arduino PID Library by Brett Beauregard, make sure to install it in your Arduino IDE.

Here is a simple sketch to follow a single ArUco marker and to attempt to stay at a fixed distance from it:

// ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// JeVois Smart Embedded Machine Vision Toolkit - Copyright (C) 2017 by Laurent Itti, the University of Southern
// California (USC), and iLab at USC. See http://iLab.usc.edu and http://jevois.org for information about this project.
//
// This file is part of the JeVois Smart Embedded Machine Vision Toolkit.  This program is free software; you can
// redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software
// Foundation, version 2.  This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
// without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public
// License for more details.  You should have received a copy of the GNU General Public License along with this program;
// if not, write to the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
//
// Contact information: Laurent Itti - 3641 Watt Way, HNB-07A - Los Angeles, CA 90089-2520 - USA.
// Tel: +1 213 740 3527 - itti@pollux.usc.edu - http://iLab.usc.edu - http://jevois.org
// ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
// You need to install the Arduino PID Library by Brett Beauregard
#include <PID_v1.h>
#include <Servo.h>
 
// Pins setup: Use RXLED(17) on Arduino pro micro 32u4 which does not have LED(13)
#define LPWMPIN 3
#define LIN1PIN 2
#define LIN2PIN 21
#define RPWMPIN 5
#define RIN1PIN 20
#define RIN2PIN 4
#define LEDPIN 17
#define PANPIN 9
#define TILTPIN 10
 
// Serial port to use for JeVois:
// On chips with USB (e.g., 32u4), that usually is Serial1. On chips without USB, use Serial.
#define SERIAL Serial1
 
// Serial port for debugging (optional):
#define DBGSERIAL Serial
 
// Buffer for received serial port bytes:
#define INLEN 128
char instr[INLEN + 1];
 
// Gain values to compensate for unbalanced motors: Do not exceed 655. Give a lower value to the motor that is faster.
// E.g., if your car vires to the left when both motors are 100% forward, your right motor is a bit faster, so decrease
// rightgain to compensate.
long leftgain = 655;
long rightgain = 655;
 
// Desired target width in standardized JeVois coordinates (a value of 2000 would occupy the whole field of view):
int TARGW = 300;
 
// Create a PID for target width, and one for steering angle:
double wset, win, wout, aset, ain, aout;
PID wpid(&win, &wout, &wset, 1.0, 0.01, 0.01, DIRECT);
PID apid(&ain, &aout, &aset, 0.5, 0.05, 0.02, DIRECT);
 
// current speed and steering:
double speed = 0;
double steer = 0;
 
int ledstate = 0;
 
// Pan and tilt servos zero values and +/- angular range, in degrees:
#define PANZERO 90
#define PANRANGE 60
#define TILTZERO 70
#define TILTRANGE 40
 
// ###################################################################################################
// Simple PD servo controller
class ServoPD
{
public:
  ServoPD(long Kp, long Kd, long zero, long range, long scalebits = 8) :
  itsKp(Kp), itsKd(Kd), itsPos(zero << scalebits),
  itsPrevTarget(zero << scalebits), itsZero(zero << scalebits),
  itsRange(range << scalebits), itsScaleBits(scalebits)
  { }
 
  void attach(int pin, int pos)
  {
    itsServo.attach(pin);
    itsPos = (pos << itsScaleBits); 
    itsServo.write(pos);
  }
  
  long get() const
  {
    return (itsPos >> itsScaleBits);
  }
  
  void update(long targetpos)
  {
    targetpos <<= itsScaleBits;
    long diff = itsKp * targetpos + itsKd * (targetpos - itsPrevTarget);
    itsPos += (diff >> 16);
    itsPos = constrain(itsPos, itsZero - itsRange, itsZero + itsRange);
    itsServo.write(itsPos >> itsScaleBits);
    itsPrevTarget = targetpos;
  }
 
  void reset(long targetpos)
  {
    targetpos <<= itsScaleBits;
    itsPos = constrain(itsPos, itsZero - itsRange, itsZero + itsRange);
    itsPrevTarget = targetpos;
  }
  
  long rawget()
  {
    return itsPos >> itsScaleBits;
  }
 
  void rawset(long rawval)
  {
    itsPos = rawval << itsScaleBits;
    itsServo.write(rawval);
  }
  
  private:
  Servo itsServo;
  long itsKp, itsKd, itsPos, itsPrevTarget, itsZero, itsRange, itsScaleBits;
};
 
// Create one servo PD controler for camera pan and another for camera tilt:
ServoPD panservo(400, 200, PANZERO, PANRANGE);
ServoPD tiltservo(300, 100, TILTZERO, TILTRANGE);
 
// ###################################################################################################
void setup()
{
  SERIAL.setTimeout(50);
  SERIAL.begin(115200);
  SERIAL.setTimeout(50);
  pinMode(LPWMPIN, OUTPUT);
  pinMode(LIN1PIN, OUTPUT);
  pinMode(LIN2PIN, OUTPUT);
  pinMode(RPWMPIN, OUTPUT);
  pinMode(RIN1PIN, OUTPUT);
  pinMode(RIN2PIN, OUTPUT);
  pinMode(LEDPIN, OUTPUT);
  pinMode(PANPIN, OUTPUT);
  pinMode(TILTPIN, OUTPUT);
  motor(0, 0);
 
  panservo.attach(PANPIN, PANZERO);
  tiltservo.attach(TILTPIN, TILTZERO);
 
  win = TARGW; wset = TARGW; wout = 0;
  wpid.SetMode(AUTOMATIC);
  wpid.SetSampleTime(10);
  wpid.SetOutputLimits(-120, 120);
  
  ain = 0; aset = 0; aout = 0;
  apid.SetMode(AUTOMATIC);
  apid.SetSampleTime(10);
  apid.SetOutputLimits(-50, 50);
}
 
// ###################################################################################################
void loop()
{
  byte len = SERIAL.readBytesUntil('\n', instr, INLEN);
  instr[len] = 0;
 
  char * tok = strtok(instr, " \r\n");
  int state = 0; int id, targx, targy, targw, targh;
 
  while (tok)
  {
    // State machine:
    // 0: start parsing
    // 1: N2 command, parse id
    // 2: N2 command, parse targx
    // 3: N2 command, parse targy
    // 4: N2 command, parse targw
    // 5: N2 command, parse targh
    // 6: N2 command complete
    // 1000: unknown command
    switch (state)
    {
      case 0: if (strcmp(tok, "N2") == 0) state = 1; else state = 1000; break;
      case 1: id = atoi(&tok[1]); state = 2; break; // ignore prefix
      case 2: targx = atoi(tok); state = 3; break;
      case 3: targy = atoi(tok); state = 4; break;
      case 4: targw = atoi(tok); state = 5; break;
      case 5: targh = atoi(tok); state = 6; break;
      default: break; // Skip any additional tokens
    }
    tok = strtok(0, " \r\n");
  }
 
  // If a complete new N2 command was received, act:
  if (state == 6)
  {
    // Actuate the pan/tilt head to track the target:
    panservo.update(-targx);
    tiltservo.update(targy);
 
    // Move the car forward/backward to track the derired target width:
    win = targw; wpid.Compute(); speed = wout;
    
    // Steer the car to zero out the camera's pan angle:
    ain = panservo.get() - PANZERO; apid.Compute(); steer = aout * (1.0 + fabs(speed) * 0.05);
  }
  else
  {
    // Slow down if we lost track:
    speed *= 0.95;
    steer *= 0.8;
  }
  
  // Actuate the motors:
  motor(speed + steer, speed - steer);
 
  // Blink the LED on every loop:
  digitalWrite(LEDPIN, ledstate);
  ledstate = !ledstate;
}
 
// ###################################################################################################
// Actuate the motors: Values in [-100 .. 100]; positive = forward, negative = backward, 0 = brake
void motor(long left, long right)
{
  if (left > 100) left = 100; else if (left < -100) left = -100;
  if (right > 100) right = 100; else if (right < -100) right = -100;
  
  long motleft = (left * leftgain) >> 8;
  long motright = (right * rightgain) >> 8;
  
  if (motleft > 0) {
      digitalWrite(LIN1PIN, LOW);
      digitalWrite(LIN2PIN, HIGH);
      analogWrite(LPWMPIN, motleft);
  } else if (motleft < 0) {
      digitalWrite(LIN1PIN, HIGH);
      digitalWrite(LIN2PIN, LOW);
      analogWrite(LPWMPIN, -motleft);
  } else {
      digitalWrite(LIN1PIN, LOW);
      digitalWrite(LIN2PIN, LOW);
  }
  
  if (motright > 0) {
      digitalWrite(RIN1PIN, LOW);
      digitalWrite(RIN2PIN, HIGH);
      analogWrite(RPWMPIN, motright);
  } else if (motright < 0) {
      digitalWrite(RIN1PIN, HIGH);
      digitalWrite(RIN2PIN, LOW);
      analogWrite(RPWMPIN, -motright);
  } else {
      digitalWrite(RIN1PIN, LOW);
      digitalWrite(RIN2PIN, LOW);
  }
}
 

Hopping from one target to the next

We modify the code slightly to handle multiple targets. When we receive messages from JeVois about multiple targets, we keep track of the current ArUco ID that is tracked, and the next one that is being detected. Once the currently tracked one is close to us, we switch to tracking the next one. See the slightly modified logic in the loop() function of the modified code:

// ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// JeVois Smart Embedded Machine Vision Toolkit - Copyright (C) 2017 by Laurent Itti, the University of Southern
// California (USC), and iLab at USC. See http://iLab.usc.edu and http://jevois.org for information about this project.
//
// This file is part of the JeVois Smart Embedded Machine Vision Toolkit.  This program is free software; you can
// redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software
// Foundation, version 2.  This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
// without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public
// License for more details.  You should have received a copy of the GNU General Public License along with this program;
// if not, write to the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
//
// Contact information: Laurent Itti - 3641 Watt Way, HNB-07A - Los Angeles, CA 90089-2520 - USA.
// Tel: +1 213 740 3527 - itti@pollux.usc.edu - http://iLab.usc.edu - http://jevois.org
// ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
// You need to install the Arduino PID Library by Brett Beauregard
#include <PID_v1.h>
#include <Servo.h>
 
// Pins setup: Use RXLED(17) on Arduino pro micro 32u4 which does not have LED(13)
#define LPWMPIN 3
#define LIN1PIN 2
#define LIN2PIN 21
#define RPWMPIN 5
#define RIN1PIN 20
#define RIN2PIN 4
#define LEDPIN 17
#define PANPIN 9
#define TILTPIN 10
 
// Serial port to use for JeVois:
// On chips with USB (e.g., 32u4), that usually is Serial1. On chips without USB, use Serial.
#define SERIAL Serial1
 
// Serial port for debugging (optional):
#define DBGSERIAL Serial
 
// Buffer for received serial port bytes:
#define INLEN 128
char instr[INLEN + 1];
 
// Gain values to compensate for unbalanced motors: Do not exceed 655. Give a lower value to the motor that is faster.
// E.g., if your car vires to the left when both motors are 100% forward, your right motor is a bit faster, so decrease
// rightgain to compensate.
long leftgain = 655;
long rightgain = 655;
 
// Desired target width in standardized JeVois coordinates (a value of 2000 would occupy the whole field of view):
int TARGW = 300;
 
// Create a PID for target width, and one for steering angle:
double wset, win, wout, aset, ain, aout;
PID wpid(&win, &wout, &wset, 1.0, 0.01, 0.01, DIRECT);
PID apid(&ain, &aout, &aset, 0.5, 0.05, 0.02, DIRECT);
 
// current speed and steering:
double speed = 0;
double steer = 0;
int steerdelay = 0; // Delay in steering when re-aquiring a target to avoid steering jerk
 
// Pan and tilt servos zero values and +/- angular range, in degrees:
#define PANZERO 90
#define PANRANGE 60
#define TILTZERO 70
#define TILTRANGE 40
 
// Handoff between several targets:
int currid = -1, currw = 0, currt = 0, nextid = -1, nextw = 0, nextt = 0, doneid = -1;
#define HANDOFFW ((TARGW * 5) / 10)
int t = 0;
 
// ###################################################################################################
// Simple PD servo controller
class ServoPD
{
public:
  ServoPD(long Kp, long Kd, long zero, long range, long scalebits = 8) :
  itsKp(Kp), itsKd(Kd), itsPos(zero << scalebits),
  itsPrevTarget(zero << scalebits), itsZero(zero << scalebits),
  itsRange(range << scalebits), itsScaleBits(scalebits)
  { }
 
  void attach(int pin, int pos)
  {
    itsServo.attach(pin);
    itsPos = (pos << itsScaleBits); 
    itsServo.write(pos);
  }
  
  long get() const
  {
    return (itsPos >> itsScaleBits);
  }
  
  void update(long targetpos)
  {
    targetpos <<= itsScaleBits;
    long diff = itsKp * targetpos + itsKd * (targetpos - itsPrevTarget);
    itsPos += (diff >> 16);
    itsPos = constrain(itsPos, itsZero - itsRange, itsZero + itsRange);
    itsServo.write(itsPos >> itsScaleBits);
    itsPrevTarget = targetpos;
  }
 
  void reset(long targetpos)
  {
    targetpos <<= itsScaleBits;
    itsPos = constrain(itsPos, itsZero - itsRange, itsZero + itsRange);
    itsPrevTarget = targetpos;
  }
  
  long rawget()
  {
    return itsPos >> itsScaleBits;
  }
 
  void rawset(long rawval)
  {
    itsPos = rawval << itsScaleBits;
    itsServo.write(rawval);
  }
  
  private:
  Servo itsServo;
  long itsKp, itsKd, itsPos, itsPrevTarget, itsZero, itsRange, itsScaleBits;
};
 
// Create one servo PD controler for camera pan and another for camera tilt:
//ServoPD panservo(400, 200, PANZERO, PANRANGE);
ServoPD panservo(100, 50, PANZERO, PANRANGE);
ServoPD tiltservo(300, 100, TILTZERO, TILTRANGE);
 
// ###################################################################################################
void setup()
{
  SERIAL.setTimeout(50);
  SERIAL.begin(115200);
  SERIAL.setTimeout(50);
  pinMode(LPWMPIN, OUTPUT);
  pinMode(LIN1PIN, OUTPUT);
  pinMode(LIN2PIN, OUTPUT);
  pinMode(RPWMPIN, OUTPUT);
  pinMode(RIN1PIN, OUTPUT);
  pinMode(RIN2PIN, OUTPUT);
  pinMode(LEDPIN, OUTPUT);
  pinMode(PANPIN, OUTPUT);
  pinMode(TILTPIN, OUTPUT);
  motor(0, 0);
 
  panservo.attach(PANPIN, PANZERO);
  tiltservo.attach(TILTPIN, TILTZERO);
 
  win = TARGW; wset = TARGW; wout = 0;
  wpid.SetMode(AUTOMATIC);
  wpid.SetSampleTime(10);
  wpid.SetOutputLimits(-120, 120);
  
  ain = 0; aset = 0; aout = 0;
  apid.SetMode(AUTOMATIC);
  apid.SetSampleTime(10);
  apid.SetOutputLimits(-50, 50);
}
 
// ###################################################################################################
void loop()
{
  byte len = SERIAL.readBytesUntil('\n', instr, INLEN);
  instr[len] = 0;
 
  char * tok = strtok(instr, " \r\n");
  int state = 0; int id, targx, targy, targw, targh;
 
  while (tok)
  {
    // State machine:
    // 0: start parsing
    // 1: N2 command, parse id
    // 2: N2 command, parse targx
    // 3: N2 command, parse targy
    // 4: N2 command, parse targw
    // 5: N2 command, parse targh
    // 6: N2 command complete
    // 1000: unknown command
    switch (state)
    {
      case 0: if (strcmp(tok, "N2") == 0) state = 1; else state = 1000; break;
      case 1: id = atoi(&tok[1]); state = 2; break; // ignore prefix
      case 2: targx = atoi(tok); state = 3; break;
      case 3: targy = atoi(tok); state = 4; break;
      case 4: targw = atoi(tok); state = 5; break;
      case 5: targh = atoi(tok); state = 6; break;
      default: break; // Skip any additional tokens
    }
    tok = strtok(0, " \r\n");
  }
 
  // If a complete new N2 command was received, act:
  if (state == 6)
  {
    // Update our steering delay after re-aquisition of previously lost target:
    if (steerdelay) steerdelay -= 1;
    
    // Decide whether we should track this target or another (larger) one:
    int trackit = 0;
 
    // If we are not tracking anything, track this one:
    if (currid == -1)
    { currid = id; currw = targw; currt = t; trackit = 1; } // New target, track it
    else
    {
      if (id == doneid)
      { trackit = 0; } // Ignore this one as we have already tracked it all the way
      else if (id == currid)
      { currw = targw; currt = t; trackit = 1; } // Keep tracking current target
      else if (nextid == -1 && targw < currw)
      { nextid = id; nextw = targw; nextt = t; trackit = 0; } // Next target detected, do not track it yet
      else if (id == nextid && currw > HANDOFFW)
      { doneid = currid; currid = nextid; nextid = -1; currw = targw; currt = t; trackit = 1; } // handoff
    }
 
    if (trackit)
    {
      // Actuate the pan/tilt head to track the target:
      panservo.update(-targx);
      tiltservo.update(targy);
 
      // Move the car forward/backward to track the derired target width:
      win = targw; wpid.Compute(); speed = wout;
    
      // Steer the car to zero out the camera's pan angle:
      ain = panservo.get() - PANZERO; apid.Compute(); steer = aout * (1.0 + fabs(speed) * 0.05);
    }
  }
  else
  {
    // Slow down if we lost track:
    speed *= 0.95;
    steer *= 0.8;
    steerdelay = 1;
  }
  
  // Actuate the motors:
  if (steerdelay) steer = 0.0; // delay steering a bit after target re-acquisition to avoid steering jerk
  motor(speed + steer, speed - steer);
 
  // Invalidate previously detected targets that we have not seen in a while:  
  if (t - currt > 30) { currid = -1; nextid = -1; doneid = -1; }
  if (t - nextt > 30) nextid = -1;
  
  // Blink the LED on every loop:
  digitalWrite(LEDPIN, t & 1);
  t += 1;
}
 
// ###################################################################################################
// Actuate the motors: Values in [-100 .. 100]; positive = forward, negative = backward, 0 = brake
void motor(long left, long right)
{
  if (left > 100) left = 100; else if (left < -100) left = -100;
  if (right > 100) right = 100; else if (right < -100) right = -100;
  
  long motleft = (left * leftgain) >> 8;
  long motright = (right * rightgain) >> 8;
  
  if (motleft > 0) {
      digitalWrite(LIN1PIN, LOW);
      digitalWrite(LIN2PIN, HIGH);
      analogWrite(LPWMPIN, motleft);
  } else if (motleft < 0) {
      digitalWrite(LIN1PIN, HIGH);
      digitalWrite(LIN2PIN, LOW);
      analogWrite(LPWMPIN, -motleft);
  } else {
      digitalWrite(LIN1PIN, LOW);
      digitalWrite(LIN2PIN, LOW);
  }
  
  if (motright > 0) {
      digitalWrite(RIN1PIN, LOW);
      digitalWrite(RIN2PIN, HIGH);
      analogWrite(RPWMPIN, motright);
  } else if (motright < 0) {
      digitalWrite(RIN1PIN, HIGH);
      digitalWrite(RIN2PIN, LOW);
      analogWrite(RPWMPIN, -motright);
  } else {
      digitalWrite(RIN1PIN, LOW);
      digitalWrite(RIN2PIN, LOW);
  }
}