Tag Archives: Control System

In this Video Lesson we show an initial control system that allows us to position a camera on a pan/tilt servo system to keep an object of interest in the center of the frame. The pan/tilt servo hat will continuously adjust so that the object we are tracking remains in the center of the frame. In this example we are only tracking in the ‘pan’ direction. It is left as a homework assignment for the student to extend the software to also track in the tilt direction. This should be a straightforward extension to our pan example.

import cv2
from picamera2 import Picamera2
import time
import numpy as np
from servo import Servo
picam2 = Picamera2()

pan=Servo(pin=13)
tilt=Servo(pin=12)

panAngle=0
tiltAngle=0

pan.set_angle(panAngle)
tilt.set_angle(tiltAngle)

dispW=1280
dispH=720
picam2.preview_configuration.main.size = (dispW,dispH)
picam2.preview_configuration.main.format = "RGB888"
picam2.preview_configuration.controls.FrameRate=30
picam2.preview_configuration.align()
picam2.configure("preview")
picam2.start()
fps=0
pos=(30,60)
font=cv2.FONT_HERSHEY_SIMPLEX
height=1.5
weight=3
myColor=(0,0,255)

def onTrack1(val):
    global hueLow
    hueLow=val
    print('Hue Low',hueLow)
def onTrack2(val):
    global hueHigh
    hueHigh=val
    print('Hue High',hueHigh)
def onTrack3(val):
    global satLow
    satLow=val
    print('Sat Low',satLow)
def onTrack4(val):
    global satHigh
    satHigh=val
    print('Sat High',satHigh)
def onTrack5(val):
    global valLow
    valLow=val
    print('Val Low',valLow)
def onTrack6(val):
    global valHigh
    valHigh=val
    print('Val High',valHigh)

cv2.namedWindow('myTracker')

cv2.createTrackbar('Hue Low','myTracker',10,179,onTrack1)
cv2.createTrackbar('Hue High','myTracker',20,179,onTrack2)
cv2.createTrackbar('Sat Low','myTracker',100,255,onTrack3)
cv2.createTrackbar('Sat High','myTracker',255,255,onTrack4)
cv2.createTrackbar('Val Low','myTracker',100,255,onTrack5)
cv2.createTrackbar('Val High','myTracker',255,255,onTrack6)


while True:
    tStart=time.time()
    frame= picam2.capture_array()
    frame=cv2.flip(frame,-1)
    frameHSV=cv2.cvtColor(frame,cv2.COLOR_BGR2HSV)
    cv2.putText(frame,str(int(fps))+' FPS',pos,font,height,myColor,weight)
    lowerBound=np.array([hueLow,satLow,valLow])
    upperBound=np.array([hueHigh,satHigh,valHigh])
    myMask=cv2.inRange(frameHSV,lowerBound,upperBound)
    myMaskSmall=cv2.resize(myMask,(int(dispW/2),int(dispH/2)))
    myObject=cv2.bitwise_and(frame,frame, mask=myMask)
    myObjectSmall=cv2.resize(myObject,(int(dispW/2),int(dispH/2)))
    
    contours,junk=cv2.findContours(myMask,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)
    if len(contours)>0:
        contours=sorted(contours,key=lambda x:cv2.contourArea(x),reverse=True)
        #cv2.drawContours(frame,contours,-1,(255,0,0),3)
        contour=contours[0]
        x,y,w,h=cv2.boundingRect(contour)
        cv2.rectangle(frame,(x,y),(x+w,y+h),(0,0,255),3)
        error=(x+w/2)-dispW/2
        if error>30:
            panAngle=panAngle-1
            if panAngle<-90:
                panAngle=-90
            pan.set_angle(panAngle)
        if error<-30:
            panAngle=panAngle+1
            if panAngle>90:
                panAngle=90
            pan.set_angle(panAngle)
        
    cv2.imshow('Camera',frame)
    cv2.imshow('Mask',myMaskSmall)
    cv2.imshow('My Object',myObjectSmall)
    if cv2.waitKey(1)==ord('q'):
        break
    tEnd=time.time()
    loopTime=tEnd-tStart
    fps=.9*fps + .1*(1/loopTime)
cv2.destroyAllWindows()

100

101

102

103

104

105

106

107

import cv2

from picamera2 import Picamera2

import time

import numpy as np

from servo import Servo

picam2 = Picamera2()

pan=Servo(pin=13)

tilt=Servo(pin=12)

panAngle=0

tiltAngle=0

pan.set_angle(panAngle)

tilt.set_angle(tiltAngle)

dispW=1280

dispH=720

picam2.preview_configuration.main.size = (dispW,dispH)

picam2.preview_configuration.main.format = "RGB888"

picam2.preview_configuration.controls.FrameRate=30

picam2.preview_configuration.align()

picam2.configure("preview")

picam2.start()

fps=0

pos=(30,60)

font=cv2.FONT_HERSHEY_SIMPLEX

height=1.5

weight=3

myColor=(0,0,255)

def onTrack1(val):

global hueLow

hueLow=val

print('Hue Low',hueLow)

def onTrack2(val):

global hueHigh

hueHigh=val

print('Hue High',hueHigh)

def onTrack3(val):

global satLow

satLow=val

print('Sat Low',satLow)

def onTrack4(val):

global satHigh

satHigh=val

print('Sat High',satHigh)

def onTrack5(val):

global valLow

valLow=val

print('Val Low',valLow)

def onTrack6(val):

global valHigh

valHigh=val

print('Val High',valHigh)

cv2.namedWindow('myTracker')

cv2.createTrackbar('Hue Low','myTracker',10,179,onTrack1)

cv2.createTrackbar('Hue High','myTracker',20,179,onTrack2)

cv2.createTrackbar('Sat Low','myTracker',100,255,onTrack3)

cv2.createTrackbar('Sat High','myTracker',255,255,onTrack4)

cv2.createTrackbar('Val Low','myTracker',100,255,onTrack5)

cv2.createTrackbar('Val High','myTracker',255,255,onTrack6)

while True:

tStart=time.time()

frame= picam2.capture_array()

frame=cv2.flip(frame,-1)

frameHSV=cv2.cvtColor(frame,cv2.COLOR_BGR2HSV)

cv2.putText(frame,str(int(fps))+' FPS',pos,font,height,myColor,weight)

lowerBound=np.array([hueLow,satLow,valLow])

upperBound=np.array([hueHigh,satHigh,valHigh])

myMask=cv2.inRange(frameHSV,lowerBound,upperBound)

myMaskSmall=cv2.resize(myMask,(int(dispW/2),int(dispH/2)))

myObject=cv2.bitwise_and(frame,frame, mask=myMask)

myObjectSmall=cv2.resize(myObject,(int(dispW/2),int(dispH/2)))

contours,junk=cv2.findContours(myMask,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)

if len(contours)>0:

contours=sorted(contours,key=lambda x:cv2.contourArea(x),reverse=True)

#cv2.drawContours(frame,contours,-1,(255,0,0),3)

contour=contours[0]

x,y,w,h=cv2.boundingRect(contour)

cv2.rectangle(frame,(x,y),(x+w,y+h),(0,0,255),3)

error=(x+w/2)-dispW/2

if error>30:

panAngle=panAngle-1

if panAngle<-90:

panAngle=-90

pan.set_angle(panAngle)

if error<-30:

panAngle=panAngle+1

if panAngle>90:

panAngle=90

pan.set_angle(panAngle)

cv2.imshow('Camera',frame)

cv2.imshow('Mask',myMaskSmall)

cv2.imshow('My Object',myObjectSmall)

if cv2.waitKey(1)==ord('q'):

break

tEnd=time.time()

loopTime=tEnd-tStart

fps=.9*fps + .1*(1/loopTime)

cv2.destroyAllWindows()

Arduino 9-Axis IMU Totorials

9-Axis IMU LESSON 26: Understanding PID Control systems with Arduino

February 6, 2020 admin

In this lesson we use our BNO055 9-axis sensor, and our pan-tilt servo mount to create a self-leveling platform based on a classic PID control system. The video takes you step by step through the theory behind a PID controller, and then demonstrates a practical example in hardware. In the video we develop the code below.

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <math.h>
#include <Servo.h>

Servo pitchServo;
Servo rollServo;

float q0;
float q1;
float q2;
float q3;

float k1=.5;
float k2=55;
float k3=.00001;

int milliOld;
int milliNew;
int dt;

float rollTarget=0;
float rollActual;
float rollError=0;
float rollErrorOld;
float rollErrorChange;
float rollErrorSlope=0;
float rollErrorArea=0;
float rollServoVal=90;

float pitchTarget=0;
float pitchActual;
float pitchError=0;
float pitchErrorOld;
float pitchErrorChange;
float pitchErrorSlope=0;
float pitchErrorArea=0;
float pitchServoVal=90;

#define BNO055_SAMPLERATE_DELAY_MS (100)

Adafruit_BNO055 myIMU = Adafruit_BNO055();

void setup() {
  // put your setup code here, to run once:
Serial.begin(115200);
myIMU.begin();
delay(1000);
int8_t temp=myIMU.getTemp();
myIMU.setExtCrystalUse(true);
rollServo.attach(2);
pitchServo.attach(3);

rollServo.write(rollServoVal);
delay(20);
pitchServo.write(pitchServoVal);
delay(20);

milliNew=millis();
}

void loop() {
  // put your main code here, to run repeatedly:
uint8_t system, gyro, accel, mg = 0;
myIMU.getCalibration(&system, &gyro, &accel, &mg);

imu::Quaternion quat=myIMU.getQuat();

q0=quat.w();
q1=quat.x();
q2=quat.y();
q3=quat.z();

rollActual=atan2(2*(q0*q1+q2*q3),1-2*(q1*q1+q2*q2));
pitchActual=asin(2*(q0*q2-q3*q1));

rollActual=rollActual/(2*3.141592654)*360;
pitchActual=pitchActual/(2*3.141592654)*360;

milliOld=milliNew;
milliNew=millis();
dt=milliNew-milliOld;

rollErrorOld=rollError;
rollError=rollTarget-rollActual;
rollErrorChange=rollError-rollErrorOld;
rollErrorSlope=rollErrorChange/dt;
rollErrorArea=rollErrorArea+rollError*dt;

pitchErrorOld=pitchError;
pitchError=pitchTarget-pitchActual;
pitchErrorChange=pitchError-pitchErrorOld;
pitchErrorSlope=pitchErrorChange/dt;
pitchErrorArea=pitchErrorArea+pitchError*dt;

rollServoVal=rollServoVal+k1*rollError+k2*rollErrorSlope+k3*rollErrorArea;
rollServo.write(rollServoVal);

pitchServoVal=pitchServoVal+k1*pitchError+k2*pitchErrorSlope+k3*pitchErrorArea;
pitchServo.write(pitchServoVal);

Serial.print(rollTarget);
Serial.print(",");
Serial.print(rollActual);
Serial.print(",");
Serial.print(pitchTarget);
Serial.print(",");
Serial.print(pitchActual);
Serial.print(",");
Serial.print(accel);
Serial.print(",");
Serial.print(gyro);
Serial.print(",");
Serial.print(mg);
Serial.print(",");
Serial.println(system);

delay(BNO055_SAMPLERATE_DELAY_MS);
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

#include <Wire.h>

#include <Adafruit_Sensor.h>

#include <Adafruit_BNO055.h>

#include <utility/imumaths.h>

#include <math.h>

#include <Servo.h>

Servo pitchServo;

Servo rollServo;

float q0;

float q1;

float q2;

float q3;

float k1=.5;

float k2=55;

float k3=.00001;

int milliOld;

int milliNew;

int dt;

float rollTarget=0;

float rollActual;

float rollError=0;

float rollErrorOld;

float rollErrorChange;

float rollErrorSlope=0;

float rollErrorArea=0;

float rollServoVal=90;

float pitchTarget=0;

float pitchActual;

float pitchError=0;

float pitchErrorOld;

float pitchErrorChange;

float pitchErrorSlope=0;

float pitchErrorArea=0;

float pitchServoVal=90;

#define BNO055_SAMPLERATE_DELAY_MS (100)

Adafruit_BNO055 myIMU = Adafruit_BNO055();

void setup() {

// put your setup code here, to run once:

Serial.begin(115200);

myIMU.begin();

delay(1000);

int8_t temp=myIMU.getTemp();

myIMU.setExtCrystalUse(true);

rollServo.attach(2);

pitchServo.attach(3);

rollServo.write(rollServoVal);

delay(20);

pitchServo.write(pitchServoVal);

delay(20);

milliNew=millis();

}

void loop() {

// put your main code here, to run repeatedly:

uint8_t system, gyro, accel, mg = 0;

myIMU.getCalibration(&system, &gyro, &accel, &mg);

imu::Quaternion quat=myIMU.getQuat();

q0=quat.w();

q1=quat.x();

q2=quat.y();

q3=quat.z();

rollActual=atan2(2*(q0*q1+q2*q3),1-2*(q1*q1+q2*q2));

pitchActual=asin(2*(q0*q2-q3*q1));

rollActual=rollActual/(2*3.141592654)*360;

pitchActual=pitchActual/(2*3.141592654)*360;

milliOld=milliNew;

milliNew=millis();

dt=milliNew-milliOld;

rollErrorOld=rollError;

rollError=rollTarget-rollActual;

rollErrorChange=rollError-rollErrorOld;

rollErrorSlope=rollErrorChange/dt;

rollErrorArea=rollErrorArea+rollError*dt;

pitchErrorOld=pitchError;

pitchError=pitchTarget-pitchActual;

pitchErrorChange=pitchError-pitchErrorOld;

pitchErrorSlope=pitchErrorChange/dt;

pitchErrorArea=pitchErrorArea+pitchError*dt;

rollServoVal=rollServoVal+k1*rollError+k2*rollErrorSlope+k3*rollErrorArea;

rollServo.write(rollServoVal);

pitchServoVal=pitchServoVal+k1*pitchError+k2*pitchErrorSlope+k3*pitchErrorArea;

pitchServo.write(pitchServoVal);

Serial.print(rollTarget);

Serial.print(",");

Serial.print(rollActual);

Serial.print(",");

Serial.print(pitchTarget);

Serial.print(",");

Serial.print(pitchActual);

Serial.print(",");

Serial.print(accel);

Serial.print(",");

Serial.print(gyro);

Serial.print(",");

Serial.print(mg);

Serial.print(",");

Serial.println(system);

delay(BNO055_SAMPLERATE_DELAY_MS);

}

Arduino, Arduino 9-Axis IMU Totorials, Tutorial

9-Axis IMU LESSON 24: How To Build a Self Leveling Platform with Arduino

January 23, 2020 admin

In this lesson we develop the initial simple code for leveling the platform. We implement the simplest control system, which is to just increment or decrement the servos based on whether the actual position is greater or less than the target position. We increment/decrement by one degree, since this is the smallest allowed change in servo position. Next week we will implement a more sophisticated control system. Below is the arduino code developed in this lesson.

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <math.h>
#include <Servo.h>

Servo pitchServo;
Servo rollServo;

float q0;
float q1;
float q2;
float q3;

float rollTarget=0;
float rollActual;
float rollError;
float rollServoVal=90;

float pitchTarget=0;
float pitchActual;
float pitchError;
float pitchServoVal=90;

#define BNO055_SAMPLERATE_DELAY_MS (100)

Adafruit_BNO055 myIMU = Adafruit_BNO055();

void setup() {
  // put your setup code here, to run once:
Serial.begin(115200);
myIMU.begin();
delay(1000);
int8_t temp=myIMU.getTemp();
myIMU.setExtCrystalUse(true);
rollServo.attach(2);
pitchServo.attach(3);

rollServo.write(rollServoVal);
delay(20);
pitchServo.write(pitchServoVal);
delay(20);
}

void loop() {
  // put your main code here, to run repeatedly:
uint8_t system, gyro, accel, mg = 0;
myIMU.getCalibration(&system, &gyro, &accel, &mg);

imu::Quaternion quat=myIMU.getQuat();

q0=quat.w();
q1=quat.x();
q2=quat.y();
q3=quat.z();

rollActual=atan2(2*(q0*q1+q2*q3),1-2*(q1*q1+q2*q2));
pitchActual=asin(2*(q0*q2-q3*q1));

rollActual=rollActual/(2*3.141592654)*360;
pitchActual=pitchActual/(2*3.141592654)*360;

rollError=rollTarget-rollActual;
pitchError=pitchTarget-pitchActual;

if (pitchError>1.5){
  pitchServoVal=pitchServoVal+1;
  pitchServo.write(pitchServoVal);
  delay(20);
}
if (pitchError<-1.5){
  pitchServoVal=pitchServoVal-1;
  pitchServo.write(pitchServoVal);
  delay(20);
}

if (rollError>1.5){
  rollServoVal=rollServoVal+1;
  rollServo.write(rollServoVal);
  delay(20);
}
if (rollError<-1.5){
  rollServoVal=rollServoVal-1;
  rollServo.write(rollServoVal);
  delay(20);
}

Serial.print(rollTarget);
Serial.print(",");
Serial.print(rollActual);
Serial.print(",");
Serial.print(pitchTarget);
Serial.print(",");
Serial.print(pitchActual);
Serial.print(",");
Serial.print(accel);
Serial.print(",");
Serial.print(gyro);
Serial.print(",");
Serial.print(mg);
Serial.print(",");
Serial.println(system);

delay(BNO055_SAMPLERATE_DELAY_MS);
}

100

101

102

103

104

105

106

#include <Wire.h>

#include <Adafruit_Sensor.h>

#include <Adafruit_BNO055.h>

#include <utility/imumaths.h>

#include <math.h>

#include <Servo.h>

Servo pitchServo;

Servo rollServo;

float q0;

float q1;

float q2;

float q3;

float rollTarget=0;

float rollActual;

float rollError;

float rollServoVal=90;

float pitchTarget=0;

float pitchActual;

float pitchError;

float pitchServoVal=90;

#define BNO055_SAMPLERATE_DELAY_MS (100)

Adafruit_BNO055 myIMU = Adafruit_BNO055();

void setup() {

// put your setup code here, to run once:

Serial.begin(115200);

myIMU.begin();

delay(1000);

int8_t temp=myIMU.getTemp();

myIMU.setExtCrystalUse(true);

rollServo.attach(2);

pitchServo.attach(3);

rollServo.write(rollServoVal);

delay(20);

pitchServo.write(pitchServoVal);

delay(20);

}

void loop() {

// put your main code here, to run repeatedly:

uint8_t system, gyro, accel, mg = 0;

myIMU.getCalibration(&system, &gyro, &accel, &mg);

imu::Quaternion quat=myIMU.getQuat();

q0=quat.w();

q1=quat.x();

q2=quat.y();

q3=quat.z();

rollActual=atan2(2*(q0*q1+q2*q3),1-2*(q1*q1+q2*q2));

pitchActual=asin(2*(q0*q2-q3*q1));

rollActual=rollActual/(2*3.141592654)*360;

pitchActual=pitchActual/(2*3.141592654)*360;

rollError=rollTarget-rollActual;

pitchError=pitchTarget-pitchActual;

if (pitchError>1.5){

pitchServoVal=pitchServoVal+1;

pitchServo.write(pitchServoVal);

delay(20);

}

if (pitchError<-1.5){

pitchServoVal=pitchServoVal-1;

pitchServo.write(pitchServoVal);

delay(20);

}

if (rollError>1.5){

rollServoVal=rollServoVal+1;

rollServo.write(rollServoVal);

delay(20);

}

if (rollError<-1.5){

rollServoVal=rollServoVal-1;

rollServo.write(rollServoVal);

delay(20);

}

Serial.print(rollTarget);

Serial.print(",");

Serial.print(rollActual);

Serial.print(",");

Serial.print(pitchTarget);

Serial.print(",");

Serial.print(pitchActual);

Serial.print(",");

Serial.print(accel);

Serial.print(",");

Serial.print(gyro);

Serial.print(",");

Serial.print(mg);

Serial.print(",");

Serial.println(system);

delay(BNO055_SAMPLERATE_DELAY_MS);

}

Arduino 9-Axis IMU Totorials

9-Axis IMU LESSON 23: Self Leveling Platform Using BNO055 and Arduino Controlled Servos

January 16, 2020 admin

<span data-mce-type="bookmark" style="display: inline-block; width: 0px; overflow: hidden; line-height: 0;" class="mce_SELRES_start"></span>In this lesson we connect the circuit and do the mechanical build for our self leveling platform. You can see a list of the gear we are using in LESSON 22.

Technology Tutorials

Tag Archives: Control System

Using a Pan/Tilt Camera Servo to Track an Object of Interest in OpenCV

9-Axis IMU LESSON 26: Understanding PID Control systems with Arduino

9-Axis IMU LESSON 24: How To Build a Self Leveling Platform with Arduino

9-Axis IMU LESSON 23: Self Leveling Platform Using BNO055 and Arduino Controlled Servos

Making The World a Better Place One High Tech Project at a Time. Enjoy!