All posts by admin

Guys in this lesson we show how you can use linear regression to more accurately calibrate turns for our robot car. We will take you through the math step-by-step, and then show how to code up the results. If you want to play along at home, you can get your Elegoo Robot Car HERE.

For your convenience, the code we developed in the lesson is included below.

int ENA=5;
int ENB=6;
int IN1=7;
int IN2=8;
int IN3=9;
int IN4=11;
float d;
int degRot;
int left;
int right;
float v;
void setup() {
  // put your setup code here, to run once:
Serial.begin(9600);
pinMode(ENA,OUTPUT);
pinMode(ENB,OUTPUT);
pinMode(IN1,OUTPUT);
pinMode(IN2,OUTPUT);
pinMode(IN3,OUTPUT);
pinMode(IN4,OUTPUT);
digitalWrite(ENA,HIGH);
digitalWrite(ENB,HIGH);
}

void loop() {
int wv;
v=1.2;
wv=(v-.35)/.0075;
left=wv;
right=wv;
setSpeed(left,right);
forward(5, v);
turnRight(180, wv);
forward(5, v);
//calR(wv);
//forward(8,v);
//v=1.5;
//wv=(v-.35)/.0075;
//left=wv;
//right=wv;
//setSpeed(left,right);
//backward(8,v);
  while(1==1){

  }
}
void setSpeed(int leftVal,int rightVal){
  analogWrite(ENA,leftVal);
  analogWrite(ENB,rightVal);
}
void forward(float d, float v){
float t;
digitalWrite(IN1,HIGH);
digitalWrite(IN2,LOW);
digitalWrite(IN3,LOW);
digitalWrite(IN4,HIGH);
t=d/v*1000;
delay(t);
stopCar();
}
void backward(float d, float v){
float t;
digitalWrite(IN1,LOW);
digitalWrite(IN2,HIGH);
digitalWrite(IN3,HIGH);
digitalWrite(IN4,LOW);
t=d/v*1000;
delay(t);
stopCar();
}
void turnRight(int deg, int wv){
  float t;
stopCar();
delay(100);
analogWrite(ENA,125);
analogWrite(ENB,125);
digitalWrite(IN1,HIGH);
digitalWrite(IN2,LOW);
digitalWrite(IN3,HIGH);
digitalWrite(IN4,LOW);
t=(deg+6)/136.29*1000.;
Serial.println(deg);
delay(t);
stopCar();
analogWrite(ENA,wv);
analogWrite(ENB,wv);
}
void turnLeft(int deg, int wv){
float t;
analogWrite(ENA,125);
analogWrite(ENB,125);
digitalWrite(IN1,LOW);
digitalWrite(IN2,HIGH);
digitalWrite(IN3,LOW);
digitalWrite(IN4,HIGH);
t=(deg+6)/136.29*1000.;
Serial.println(deg);
Serial.println(deg);
delay(t);
stopCar();
analogWrite(ENA,wv);
analogWrite(ENB,wv);
}
void stopCar(){
digitalWrite(IN1,LOW);
digitalWrite(IN2,LOW);
digitalWrite(IN3,LOW);
digitalWrite(IN4,LOW);
}

void calF(){
digitalWrite(IN1,HIGH);
digitalWrite(IN2,LOW);
digitalWrite(IN3,LOW);
digitalWrite(IN4,HIGH);
delay(5000);
stopCar();
}
void calB(){
digitalWrite(IN1,LOW);
digitalWrite(IN2,HIGH);
digitalWrite(IN3,HIGH);
digitalWrite(IN4,LOW);
delay(5000);
stopCar();
}
void calR(int wv){
stopCar();
analogWrite(ENA,125);
analogWrite(ENB,125);
digitalWrite(IN1,HIGH);
digitalWrite(IN2,LOW);
digitalWrite(IN3,HIGH);
digitalWrite(IN4,LOW);
delay(3000);
analogWrite(ENA,wv);
analogWrite(ENB,wv);
stopCar();
}
void calL(int wv){
analogWrite(ENA,125);
analogWrite(ENB,125);
digitalWrite(IN1,LOW);
digitalWrite(IN2,HIGH);
digitalWrite(IN3,LOW);
digitalWrite(IN4,HIGH);
delay(5000);
analogWrite(ENA,wv);
analogWrite(ENB,wv);
stopCar();
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

int ENA=5;

int ENB=6;

int IN1=7;

int IN2=8;

int IN3=9;

int IN4=11;

float d;

int degRot;

int left;

int right;

float v;

void setup() {

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

Serial.begin(9600);

pinMode(ENA,OUTPUT);

pinMode(ENB,OUTPUT);

pinMode(IN1,OUTPUT);

pinMode(IN2,OUTPUT);

pinMode(IN3,OUTPUT);

pinMode(IN4,OUTPUT);

digitalWrite(ENA,HIGH);

digitalWrite(ENB,HIGH);

}

void loop() {

int wv;

v=1.2;

wv=(v-.35)/.0075;

left=wv;

right=wv;

setSpeed(left,right);

forward(5, v);

turnRight(180, wv);

forward(5, v);

//calR(wv);

//forward(8,v);

//v=1.5;

//wv=(v-.35)/.0075;

//left=wv;

//right=wv;

//setSpeed(left,right);

//backward(8,v);

while(1==1){

}

void setSpeed(int leftVal,int rightVal){

analogWrite(ENA,leftVal);

analogWrite(ENB,rightVal);

}

void forward(float d, float v){

float t;

digitalWrite(IN1,HIGH);

digitalWrite(IN2,LOW);

digitalWrite(IN3,LOW);

digitalWrite(IN4,HIGH);

t=d/v*1000;

delay(t);

stopCar();

}

void backward(float d, float v){

float t;

digitalWrite(IN1,LOW);

digitalWrite(IN2,HIGH);

digitalWrite(IN3,HIGH);

digitalWrite(IN4,LOW);

t=d/v*1000;

delay(t);

stopCar();

}

void turnRight(int deg, int wv){

float t;

stopCar();

delay(100);

analogWrite(ENA,125);

analogWrite(ENB,125);

digitalWrite(IN1,HIGH);

digitalWrite(IN2,LOW);

digitalWrite(IN3,HIGH);

digitalWrite(IN4,LOW);

t=(deg+6)/136.29*1000.;

Serial.println(deg);

delay(t);

stopCar();

analogWrite(ENA,wv);

analogWrite(ENB,wv);

}

void turnLeft(int deg, int wv){

float t;

analogWrite(ENA,125);

analogWrite(ENB,125);

digitalWrite(IN1,LOW);

digitalWrite(IN2,HIGH);

digitalWrite(IN3,LOW);

digitalWrite(IN4,HIGH);

t=(deg+6)/136.29*1000.;

Serial.println(deg);

delay(t);

stopCar();

analogWrite(ENA,wv);

analogWrite(ENB,wv);

}

void stopCar(){

digitalWrite(IN1,LOW);

digitalWrite(IN2,LOW);

digitalWrite(IN3,LOW);

digitalWrite(IN4,LOW);

}

void calF(){

digitalWrite(IN1,HIGH);

digitalWrite(IN2,LOW);

digitalWrite(IN3,LOW);

digitalWrite(IN4,HIGH);

delay(5000);

stopCar();

}

void calB(){

digitalWrite(IN1,LOW);

digitalWrite(IN2,HIGH);

digitalWrite(IN3,HIGH);

digitalWrite(IN4,LOW);

delay(5000);

stopCar();

}

void calR(int wv){

stopCar();

analogWrite(ENA,125);

analogWrite(ENB,125);

digitalWrite(IN1,HIGH);

digitalWrite(IN2,LOW);

digitalWrite(IN3,HIGH);

digitalWrite(IN4,LOW);

delay(3000);

analogWrite(ENA,wv);

analogWrite(ENB,wv);

stopCar();

}

void calL(int wv){

analogWrite(ENA,125);

analogWrite(ENB,125);

digitalWrite(IN1,LOW);

digitalWrite(IN2,HIGH);

digitalWrite(IN3,LOW);

digitalWrite(IN4,HIGH);

delay(5000);

analogWrite(ENA,wv);

analogWrite(ENB,wv);

stopCar();

}

Jetson Xavier NX

Jetson Xavier NX Lesson 12: Intelligent Scanning for Objects of Interest

August 5, 2020 admin

In this Video Tutorial we show how a camera on a pan/tilt control system can be programmed to search for an object of interest, and then track it when found. Our system has two independent camera systems, and each can track a separate item of interest independently. The code is written in python, using the OpenCV library. The video takes you through the lesson step-by-step, and then the code is included below for your convenience.

If you want to play along at home, we are using the Jetson Xavier NX, which you can pick up HERE. You will also need to of the bracket/servo kits, which you can get HERE, and then two Raspberry Pi Version two cameras, available HERE.

import cv2
import numpy as np
import time
from adafruit_servokit import ServoKit
print(cv2.__version__)
timeMark=time.time()
dtFIL=0

scanRight=True
scanLeft=True

def nothing(x):
    pass

cv2.namedWindow('TrackBars')
cv2.moveWindow('TrackBars',1320,0)
cv2.createTrackbar('hueLower', 'TrackBars',100,179,nothing)
cv2.createTrackbar('hueUpper', 'TrackBars',116,179,nothing)
cv2.createTrackbar('satLow', 'TrackBars',160,255,nothing)
cv2.createTrackbar('satHigh', 'TrackBars',255,255,nothing)
cv2.createTrackbar('valLow', 'TrackBars',150,255,nothing)
cv2.createTrackbar('valHigh', 'TrackBars',255,255,nothing)

cv2.namedWindow('TrackBars2')
cv2.moveWindow('TrackBars2',1100,0)
cv2.createTrackbar('hueLower2', 'TrackBars2',150,179,nothing)
cv2.createTrackbar('hueUpper2', 'TrackBars2',170,179,nothing)
cv2.createTrackbar('satLow2', 'TrackBars2',160,255,nothing)
cv2.createTrackbar('satHigh2', 'TrackBars2',255,255,nothing)
cv2.createTrackbar('valLow2', 'TrackBars2',150,255,nothing)
cv2.createTrackbar('valHigh2', 'TrackBars2',255,255,nothing)


kit=ServoKit(channels=16)

tilt1=90
pan1=90
tilt2=90
pan2=90

dPan1=1
dPan2=1
dTilt1=10
dTilt2=10

kit.servo[0].angle=pan1
kit.servo[1].angle=tilt1
kit.servo[2].angle=pan2
kit.servo[3].angle=tilt2

width=720
height=480
flip=2
font=cv2.FONT_HERSHEY_SIMPLEX
camSet1='nvarguscamerasrc sensor-id=0 ee-mode=1 ee-strength=0 tnr-mode=2 tnr-strength=1 wbmode=3 ! video/x-raw(memory:NVMM), width=3264, height=2464, framerate=21/1,format=NV12 ! nvvidconv flip-method='+str(flip)+' ! video/x-raw, width='+str(width)+', height='+str(height)+', format=BGRx ! videoconvert ! video/x-raw, format=BGR ! videobalance contrast=1.3 brightness=-.2 saturation=1.2 ! appsink drop=True'
camSet2='nvarguscamerasrc sensor-id=1 ee-mode=1 ee-strength=0 tnr-mode=2 tnr-strength=1 wbmode=3 ! video/x-raw(memory:NVMM), width=3264, height=2464, framerate=21/1,format=NV12 ! nvvidconv flip-method='+str(flip)+' ! video/x-raw, width='+str(width)+', height='+str(height)+', format=BGRx ! videoconvert ! video/x-raw, format=BGR ! videobalance contrast=1.3 brightness=-.2 saturation=1.2 ! appsink drop=True'

#camSet='nvarguscamerasrc sensor-id=0 ! video/x-raw(memory:NVMM), width=3264, height=2464, framerate=21/1,format=NV12 ! nvvidconv flip-method='+str(flip)+' ! video/x-raw, width='+str(width)+', height='+str(height)+', format=BGRx ! videoconvert ! video/x-raw, format=BGR ! appsink'
#camSet ='v4l2src device=/dev/video1 ! video/x-raw,width='+str(width)+',height='+str(height)+',framerate=20/1 ! videoconvert ! appsink'
cam1=cv2.VideoCapture(camSet1)
cam2=cv2.VideoCapture(camSet2)
while True:
    _, frame1 = cam1.read()
    _, frame2 = cam2.read()

    hsv1=cv2.cvtColor(frame1,cv2.COLOR_BGR2HSV)
    hsv2=cv2.cvtColor(frame2,cv2.COLOR_BGR2HSV)

    hueLow=cv2.getTrackbarPos('hueLower', 'TrackBars')
    hueUp=cv2.getTrackbarPos('hueUpper', 'TrackBars')

    Ls=cv2.getTrackbarPos('satLow', 'TrackBars')
    Us=cv2.getTrackbarPos('satHigh', 'TrackBars')

    Lv=cv2.getTrackbarPos('valLow', 'TrackBars')
    Uv=cv2.getTrackbarPos('valHigh', 'TrackBars')

    l_b=np.array([hueLow,Ls,Lv])
    u_b=np.array([hueUp,Us,Uv])


    hueLow2=cv2.getTrackbarPos('hueLower2', 'TrackBars2')
    hueUp2=cv2.getTrackbarPos('hueUpper2', 'TrackBars2')

    Ls2=cv2.getTrackbarPos('satLow2', 'TrackBars2')
    Us2=cv2.getTrackbarPos('satHigh2', 'TrackBars2')

    Lv2=cv2.getTrackbarPos('valLow2', 'TrackBars2')
    Uv2=cv2.getTrackbarPos('valHigh2', 'TrackBars2')

    l_b2=np.array([hueLow2,Ls2,Lv2])
    u_b2=np.array([hueUp2,Us2,Uv2])

    FGmask1=cv2.inRange(hsv1,l_b,u_b)
    FGmask2=cv2.inRange(hsv2,l_b2,u_b2)

    cv2.imshow('FGmask1',FGmask1)
    cv2.moveWindow('FGmask1',0,0)

    cv2.imshow('FGmask2',FGmask2)
    cv2.moveWindow('FGmask2',350,0)

    contours1,_ = cv2.findContours(FGmask1,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)
    contours1=sorted(contours1,key=lambda x:cv2.contourArea(x),reverse=True)
    for cnt in contours1:
        area=cv2.contourArea(cnt)
        (x,y,w,h)=cv2.boundingRect(cnt)
        if area>=100:
            scanLeft=False
            cv2.rectangle(frame1,(x,y),(x+w,y+h),(0,255,255),3)
            objX=x+w/2
            objY=y+h/2
            errorPan1=objX-width/2
            errorTilt1=objY-height/2
            if abs(errorPan1)>15:
                pan1=pan1+errorPan1/40
            if abs(errorTilt1)>15:
                tilt1=tilt1-errorTilt1/40
            if pan1>180:
                pan1=180
                print('Pan Out of Range')
            if pan1<0:
                pan1=0
                print('Pan Out of Range')
            if tilt1>180:
                tilt1=180
                print('Tilt Out of Range')
            if tilt1<0:
                tilt1=0
            kit.servo[2].angle=pan1
            kit.servo[3].angle=tilt1
            break
    
    contours2,_ = cv2.findContours(FGmask2,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)
    contours2=sorted(contours2,key=lambda x:cv2.contourArea(x),reverse=True)
    for cnt in contours2:
        area=cv2.contourArea(cnt)
        (x,y,w,h)=cv2.boundingRect(cnt)
        if area>=100:
            scanRight=False
            cv2.rectangle(frame2,(x,y),(x+w,y+h),(0,255,255),3)
            objX=x+w/2
            objY=y+h/2
            errorPan2=objX-width/2
            errorTilt2=objY-height/2
            if abs(errorPan2)>15:
                pan2=pan2+errorPan2/40
            if abs(errorTilt2)>15:
                tilt2=tilt2-errorTilt2/40
            if pan2>180:
                pan2=180
                print('Pan Out of Range')
            if pan2<0:
                pan2=0
                print('Pan Out of Range')
            if tilt2>180:
                tilt2=180
                print('Tilt Out of Range')
            if tilt2<0:
                tilt2=0
            kit.servo[0].angle=pan2
            kit.servo[1].angle=tilt2            
            break
    if scanLeft==True:
        if pan1>=179:
            dPan1=abs(dPan1)*(-1)
        if pan1<=1:
            dPan1=abs(dPan1)
        if pan1>=179 or pan1<=1:
            if tilt1>=170:
                dTilt1=abs(dTilt1)*(-1)
            if tilt1<=10:
                dTilt1=abs(dTilt1)
            tilt1=tilt1+dTilt1
        pan1=pan1+dPan1
        kit.servo[2].angle=pan1
        kit.servo[3].angle=tilt1
    scanLeft=True

    if scanRight==True:
        if pan2>=179:
            dPan2=abs(dPan2)*(-1)
        if pan2<=1:
            dPan2=abs(dPan2)
        if pan2>=179 or pan2<=1:
            if tilt2>=170:
                dTilt2=abs(dTilt2)*(-1)
            if tilt2<=10:
                dTilt2=abs(dTilt2)
            tilt2=tilt2+dTilt2
        pan2=pan2+dPan2
        kit.servo[0].angle=pan2
        kit.servo[1].angle=tilt2
    scanRight=True
    
    frame3=np.hstack((frame1,frame2))
    dt=time.time()-timeMark
    timeMark=time.time()
    dtFIL=.9*dtFIL + .1*dt
    fps=1/dtFIL
    cv2.rectangle(frame3,(0,0),(150,40),(0,0,255),-1)
    cv2.putText(frame3,'fps: '+str(round(fps,1)),(0,30),font,1,(0,255,255),2)

    #cv2.imshow('myCam1',frame1)
    #cv2.imshow('myCam2',frame2)
    cv2.imshow('comboCam',frame3)
    cv2.moveWindow('comboCam',0,450)

    if cv2.waitKey(1)==ord('q'):
        break
cam1.release()
cam2.release()
cv2.destroyAllWindows()

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

import cv2

import numpy as np

import time

from adafruit_servokit import ServoKit

print(cv2.__version__)

timeMark=time.time()

dtFIL=0

scanRight=True

scanLeft=True

def nothing(x):

pass

cv2.namedWindow('TrackBars')

cv2.moveWindow('TrackBars',1320,0)

cv2.createTrackbar('hueLower', 'TrackBars',100,179,nothing)

cv2.createTrackbar('hueUpper', 'TrackBars',116,179,nothing)

cv2.createTrackbar('satLow', 'TrackBars',160,255,nothing)

cv2.createTrackbar('satHigh', 'TrackBars',255,255,nothing)

cv2.createTrackbar('valLow', 'TrackBars',150,255,nothing)

cv2.createTrackbar('valHigh', 'TrackBars',255,255,nothing)

cv2.namedWindow('TrackBars2')

cv2.moveWindow('TrackBars2',1100,0)

cv2.createTrackbar('hueLower2', 'TrackBars2',150,179,nothing)

cv2.createTrackbar('hueUpper2', 'TrackBars2',170,179,nothing)

cv2.createTrackbar('satLow2', 'TrackBars2',160,255,nothing)

cv2.createTrackbar('satHigh2', 'TrackBars2',255,255,nothing)

cv2.createTrackbar('valLow2', 'TrackBars2',150,255,nothing)

cv2.createTrackbar('valHigh2', 'TrackBars2',255,255,nothing)

kit=ServoKit(channels=16)

tilt1=90

pan1=90

tilt2=90

pan2=90

dPan1=1

dPan2=1

dTilt1=10

dTilt2=10

kit.servo[0].angle=pan1

kit.servo[1].angle=tilt1

kit.servo[2].angle=pan2

kit.servo[3].angle=tilt2

width=720

height=480

flip=2

font=cv2.FONT_HERSHEY_SIMPLEX

camSet1='nvarguscamerasrc sensor-id=0 ee-mode=1 ee-strength=0 tnr-mode=2 tnr-strength=1 wbmode=3 ! video/x-raw(memory:NVMM), width=3264, height=2464, framerate=21/1,format=NV12 ! nvvidconv flip-method='+str(flip)+' ! video/x-raw, width='+str(width)+', height='+str(height)+', format=BGRx ! videoconvert ! video/x-raw, format=BGR ! videobalance contrast=1.3 brightness=-.2 saturation=1.2 ! appsink drop=True'

camSet2='nvarguscamerasrc sensor-id=1 ee-mode=1 ee-strength=0 tnr-mode=2 tnr-strength=1 wbmode=3 ! video/x-raw(memory:NVMM), width=3264, height=2464, framerate=21/1,format=NV12 ! nvvidconv flip-method='+str(flip)+' ! video/x-raw, width='+str(width)+', height='+str(height)+', format=BGRx ! videoconvert ! video/x-raw, format=BGR ! videobalance contrast=1.3 brightness=-.2 saturation=1.2 ! appsink drop=True'

#camSet='nvarguscamerasrc sensor-id=0 ! video/x-raw(memory:NVMM), width=3264, height=2464, framerate=21/1,format=NV12 ! nvvidconv flip-method='+str(flip)+' ! video/x-raw, width='+str(width)+', height='+str(height)+', format=BGRx ! videoconvert ! video/x-raw, format=BGR ! appsink'

#camSet ='v4l2src device=/dev/video1 ! video/x-raw,width='+str(width)+',height='+str(height)+',framerate=20/1 ! videoconvert ! appsink'

cam1=cv2.VideoCapture(camSet1)

cam2=cv2.VideoCapture(camSet2)

while True:

_, frame1 = cam1.read()

_, frame2 = cam2.read()

hsv1=cv2.cvtColor(frame1,cv2.COLOR_BGR2HSV)

hsv2=cv2.cvtColor(frame2,cv2.COLOR_BGR2HSV)

hueLow=cv2.getTrackbarPos('hueLower', 'TrackBars')

hueUp=cv2.getTrackbarPos('hueUpper', 'TrackBars')

Ls=cv2.getTrackbarPos('satLow', 'TrackBars')

Us=cv2.getTrackbarPos('satHigh', 'TrackBars')

Lv=cv2.getTrackbarPos('valLow', 'TrackBars')

Uv=cv2.getTrackbarPos('valHigh', 'TrackBars')

l_b=np.array([hueLow,Ls,Lv])

u_b=np.array([hueUp,Us,Uv])

hueLow2=cv2.getTrackbarPos('hueLower2', 'TrackBars2')

hueUp2=cv2.getTrackbarPos('hueUpper2', 'TrackBars2')

Ls2=cv2.getTrackbarPos('satLow2', 'TrackBars2')

Us2=cv2.getTrackbarPos('satHigh2', 'TrackBars2')

Lv2=cv2.getTrackbarPos('valLow2', 'TrackBars2')

Uv2=cv2.getTrackbarPos('valHigh2', 'TrackBars2')

l_b2=np.array([hueLow2,Ls2,Lv2])

u_b2=np.array([hueUp2,Us2,Uv2])

FGmask1=cv2.inRange(hsv1,l_b,u_b)

FGmask2=cv2.inRange(hsv2,l_b2,u_b2)

cv2.imshow('FGmask1',FGmask1)

cv2.moveWindow('FGmask1',0,0)

cv2.imshow('FGmask2',FGmask2)

cv2.moveWindow('FGmask2',350,0)

contours1,_ = cv2.findContours(FGmask1,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)

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

for cnt in contours1:

area=cv2.contourArea(cnt)

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

if area>=100:

scanLeft=False

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

objX=x+w/2

objY=y+h/2

errorPan1=objX-width/2

errorTilt1=objY-height/2

if abs(errorPan1)>15:

pan1=pan1+errorPan1/40

if abs(errorTilt1)>15:

tilt1=tilt1-errorTilt1/40

if pan1>180:

pan1=180

print('Pan Out of Range')

if pan1<0:

pan1=0

print('Pan Out of Range')

if tilt1>180:

tilt1=180

print('Tilt Out of Range')

if tilt1<0:

tilt1=0

kit.servo[2].angle=pan1

kit.servo[3].angle=tilt1

break

contours2,_ = cv2.findContours(FGmask2,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)

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

for cnt in contours2:

area=cv2.contourArea(cnt)

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

if area>=100:

scanRight=False

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

objX=x+w/2

objY=y+h/2

errorPan2=objX-width/2

errorTilt2=objY-height/2

if abs(errorPan2)>15:

pan2=pan2+errorPan2/40

if abs(errorTilt2)>15:

tilt2=tilt2-errorTilt2/40

if pan2>180:

pan2=180

print('Pan Out of Range')

if pan2<0:

pan2=0

print('Pan Out of Range')

if tilt2>180:

tilt2=180

print('Tilt Out of Range')

if tilt2<0:

tilt2=0

kit.servo[0].angle=pan2

kit.servo[1].angle=tilt2

break

if scanLeft==True:

if pan1>=179:

dPan1=abs(dPan1)*(-1)

if pan1<=1:

dPan1=abs(dPan1)

if pan1>=179 or pan1<=1:

if tilt1>=170:

dTilt1=abs(dTilt1)*(-1)

if tilt1<=10:

dTilt1=abs(dTilt1)

tilt1=tilt1+dTilt1

pan1=pan1+dPan1

kit.servo[2].angle=pan1

kit.servo[3].angle=tilt1

scanLeft=True

if scanRight==True:

if pan2>=179:

dPan2=abs(dPan2)*(-1)

if pan2<=1:

dPan2=abs(dPan2)

if pan2>=179 or pan2<=1:

if tilt2>=170:

dTilt2=abs(dTilt2)*(-1)

if tilt2<=10:

dTilt2=abs(dTilt2)

tilt2=tilt2+dTilt2

pan2=pan2+dPan2

kit.servo[0].angle=pan2

kit.servo[1].angle=tilt2

scanRight=True

frame3=np.hstack((frame1,frame2))

dt=time.time()-timeMark

timeMark=time.time()

dtFIL=.9*dtFIL + .1*dt

fps=1/dtFIL

cv2.rectangle(frame3,(0,0),(150,40),(0,0,255),-1)

cv2.putText(frame3,'fps: '+str(round(fps,1)),(0,30),font,1,(0,255,255),2)

#cv2.imshow('myCam1',frame1)

#cv2.imshow('myCam2',frame2)

cv2.imshow('comboCam',frame3)

cv2.moveWindow('comboCam',0,450)

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

break

cam1.release()

cam2.release()

cv2.destroyAllWindows()

Uncategorized

AI on the Jetson Nano LESSON 53: Object Detection and Recognition in OpenCV

August 5, 2020 admin

In this video lesson we learn how to use the NVIDIA Jetson Inference tools for detect objects in a live video. The software developed in this lesson is included below for your convenience.

import jetson.inference
import jetson.utils
import time
import cv2
import numpy as np 

timeStamp=time.time()
fpsFilt=0
net=jetson.inference.detectNet('ssd-mobilenet-v2',threshold=.5)
dispW=1280
dispH=720
flip=2
font=cv2.FONT_HERSHEY_SIMPLEX

# Gstreamer code for improvded Raspberry Pi Camera Quality
#camSet='nvarguscamerasrc wbmode=3 tnr-mode=2 tnr-strength=1 ee-mode=2 ee-strength=1 ! video/x-raw(memory:NVMM), width=3264, height=2464, format=NV12, framerate=21/1 ! nvvidconv flip-method='+str(flip)+' ! video/x-raw, width='+str(dispW)+', height='+str(dispH)+', format=BGRx ! videoconvert ! video/x-raw, format=BGR ! videobalance contrast=1.5 brightness=-.2 saturation=1.2 ! appsink'
#cam=cv2.VideoCapture(camSet)
#cam=jetson.utils.gstCamera(dispW,dispH,'0')

cam=cv2.VideoCapture('/dev/video1')
cam.set(cv2.CAP_PROP_FRAME_WIDTH, dispW)
cam.set(cv2.CAP_PROP_FRAME_HEIGHT, dispH)

#cam=jetson.utils.gstCamera(dispW,dispH,'/dev/video1')
#display=jetson.utils.glDisplay()
#while display.IsOpen():
while True:
    #img, width, height= cam.CaptureRGBA()
    _,img = cam.read()
    height=img.shape[0]
    width=img.shape[1]

    frame=cv2.cvtColor(img,cv2.COLOR_BGR2RGBA).astype(np.float32)
    frame=jetson.utils.cudaFromNumpy(frame)

    detections=net.Detect(frame, width, height)
    for detect in detections:
        #print(detect)
        ID=detect.ClassID
        top=detect.Top
        left=detect.Left
        bottom=detect.Bottom
        right=detect.Right
        item=net.GetClassDesc(ID)
        print(item,top,left,bottom,right)
    #display.RenderOnce(img,width,height)
    dt=time.time()-timeStamp
    timeStamp=time.time()
    fps=1/dt
    fpsFilt=.9*fpsFilt + .1*fps
    #print(str(round(fps,1))+' fps')
    cv2.putText(img,str(round(fpsFilt,1))+' fps',(0,30),font,1,(0,0,255),2)
    cv2.imshow('detCam',img)
    cv2.moveWindow('detCam',0,0)
    if cv2.waitKey(1)==ord('q'):
        break
cam.release()
cv2.destroyAllWindows()

import jetson.inference

import jetson.utils

import time

import cv2

import numpy as np

timeStamp=time.time()

fpsFilt=0

net=jetson.inference.detectNet('ssd-mobilenet-v2',threshold=.5)

dispW=1280

dispH=720

flip=2

font=cv2.FONT_HERSHEY_SIMPLEX

# Gstreamer code for improvded Raspberry Pi Camera Quality

#camSet='nvarguscamerasrc wbmode=3 tnr-mode=2 tnr-strength=1 ee-mode=2 ee-strength=1 ! video/x-raw(memory:NVMM), width=3264, height=2464, format=NV12, framerate=21/1 ! nvvidconv flip-method='+str(flip)+' ! video/x-raw, width='+str(dispW)+', height='+str(dispH)+', format=BGRx ! videoconvert ! video/x-raw, format=BGR ! videobalance contrast=1.5 brightness=-.2 saturation=1.2 ! appsink'

#cam=cv2.VideoCapture(camSet)

#cam=jetson.utils.gstCamera(dispW,dispH,'0')

cam=cv2.VideoCapture('/dev/video1')

cam.set(cv2.CAP_PROP_FRAME_WIDTH, dispW)

cam.set(cv2.CAP_PROP_FRAME_HEIGHT, dispH)

#cam=jetson.utils.gstCamera(dispW,dispH,'/dev/video1')

#display=jetson.utils.glDisplay()

#while display.IsOpen():

while True:

#img, width, height= cam.CaptureRGBA()

_,img = cam.read()

height=img.shape[0]

width=img.shape[1]

frame=cv2.cvtColor(img,cv2.COLOR_BGR2RGBA).astype(np.float32)

frame=jetson.utils.cudaFromNumpy(frame)

detections=net.Detect(frame, width, height)

for detect in detections:

#print(detect)

ID=detect.ClassID

top=detect.Top

left=detect.Left

bottom=detect.Bottom

right=detect.Right

item=net.GetClassDesc(ID)

print(item,top,left,bottom,right)

#display.RenderOnce(img,width,height)

dt=time.time()-timeStamp

timeStamp=time.time()

fps=1/dt

fpsFilt=.9*fpsFilt + .1*fps

#print(str(round(fps,1))+' fps')

cv2.putText(img,str(round(fpsFilt,1))+' fps',(0,30),font,1,(0,0,255),2)

cv2.imshow('detCam',img)

cv2.moveWindow('detCam',0,0)

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

break

cam.release()

cv2.destroyAllWindows()

Elegoo Smart Car Tutorial

Robotics Training LESSON 8: Setting Speed of the Smart Car

July 30, 2020 admin

In this lesson we show how to program the speed of the Elegoo Smart Car Version 3.0 using the Infrared (IR) remote control. We take you through the process of using the remote step-by-step, and for your convenience we include the code below. If you want to play along at home, you can pick up your gear HERE.

int ENA=5;
int ENB=6;
int IN1=7;
int IN2=8;
int IN3=9;
int IN4=11;
float d;
int degRot;
int left;
int right;
int wv;
float v;
void setup() {
  // put your setup code here, to run once:
Serial.begin(9600);
pinMode(ENA,OUTPUT);
pinMode(ENB,OUTPUT);
pinMode(IN1,OUTPUT);
pinMode(IN2,OUTPUT);
pinMode(IN3,OUTPUT);
pinMode(IN4,OUTPUT);
digitalWrite(ENA,HIGH);
digitalWrite(ENB,HIGH);
}

void loop() {
v=1.5;
wv=(v-.35)/.0075;
left=wv;
right=wv;
setSpeed(left,right);
forward(8,v);
v=1.5;
wv=(v-.35)/.0075;
left=wv;
right=wv;
setSpeed(left,right);
backward(8,v);
  while(1==1){

  }
}
void setSpeed(int leftVal,int rightVal){
  analogWrite(ENA,leftVal);
  analogWrite(ENB,rightVal);
}
void forward(float d, float v){
float t;
digitalWrite(IN1,HIGH);
digitalWrite(IN2,LOW);
digitalWrite(IN3,LOW);
digitalWrite(IN4,HIGH);
t=d/v*1000;
delay(t);
stopCar();
}
void backward(float d, float v){
float t;
digitalWrite(IN1,LOW);
digitalWrite(IN2,HIGH);
digitalWrite(IN3,HIGH);
digitalWrite(IN4,LOW);
t=d/v*1000;
delay(t);
stopCar();
}
void turnRight(int deg){
  float t;
digitalWrite(IN1,HIGH);
digitalWrite(IN2,LOW);
digitalWrite(IN3,HIGH);
digitalWrite(IN4,LOW);
t=deg/345.*1000.;
Serial.println(deg);
delay(t);
stopCar();
}
void turnLeft(float deg){
  float t;
digitalWrite(IN1,LOW);
digitalWrite(IN2,HIGH);
digitalWrite(IN3,LOW);
digitalWrite(IN4,HIGH);
t=deg/345.*1000.;
Serial.println(deg);
delay(t);
stopCar();
}
void stopCar(){
digitalWrite(IN1,LOW);
digitalWrite(IN2,LOW);
digitalWrite(IN3,LOW);
digitalWrite(IN4,LOW);
}

void calF(){
digitalWrite(IN1,HIGH);
digitalWrite(IN2,LOW);
digitalWrite(IN3,LOW);
digitalWrite(IN4,HIGH);
delay(5000);
stopCar();
}
void calB(){
digitalWrite(IN1,LOW);
digitalWrite(IN2,HIGH);
digitalWrite(IN3,HIGH);
digitalWrite(IN4,LOW);
delay(5000);
stopCar();
}
void calR(){
digitalWrite(IN1,HIGH);
digitalWrite(IN2,LOW);
digitalWrite(IN3,HIGH);
digitalWrite(IN4,LOW);
delay(5000);
stopCar();
}
void calL(){
digitalWrite(IN1,LOW);
digitalWrite(IN2,HIGH);
digitalWrite(IN3,LOW);
digitalWrite(IN4,HIGH);
delay(5000);
stopCar();
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

int ENA=5;

int ENB=6;

int IN1=7;

int IN2=8;

int IN3=9;

int IN4=11;

float d;

int degRot;

int left;

int right;

int wv;

float v;

void setup() {

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

Serial.begin(9600);

pinMode(ENA,OUTPUT);

pinMode(ENB,OUTPUT);

pinMode(IN1,OUTPUT);

pinMode(IN2,OUTPUT);

pinMode(IN3,OUTPUT);

pinMode(IN4,OUTPUT);

digitalWrite(ENA,HIGH);

digitalWrite(ENB,HIGH);

}

void loop() {

v=1.5;

wv=(v-.35)/.0075;

left=wv;

right=wv;

setSpeed(left,right);

forward(8,v);

v=1.5;

wv=(v-.35)/.0075;

left=wv;

right=wv;

setSpeed(left,right);

backward(8,v);

while(1==1){

}

void setSpeed(int leftVal,int rightVal){

analogWrite(ENA,leftVal);

analogWrite(ENB,rightVal);

}

void forward(float d, float v){

float t;

digitalWrite(IN1,HIGH);

digitalWrite(IN2,LOW);

digitalWrite(IN3,LOW);

digitalWrite(IN4,HIGH);

t=d/v*1000;

delay(t);

stopCar();

}

void backward(float d, float v){

float t;

digitalWrite(IN1,LOW);

digitalWrite(IN2,HIGH);

digitalWrite(IN3,HIGH);

digitalWrite(IN4,LOW);

t=d/v*1000;

delay(t);

stopCar();

}

void turnRight(int deg){

float t;

digitalWrite(IN1,HIGH);

digitalWrite(IN2,LOW);

digitalWrite(IN3,HIGH);

digitalWrite(IN4,LOW);

t=deg/345.*1000.;

Serial.println(deg);

delay(t);

stopCar();

}

void turnLeft(float deg){

float t;

digitalWrite(IN1,LOW);

digitalWrite(IN2,HIGH);

digitalWrite(IN3,LOW);

digitalWrite(IN4,HIGH);

t=deg/345.*1000.;

Serial.println(deg);

delay(t);

stopCar();

}

void stopCar(){

digitalWrite(IN1,LOW);

digitalWrite(IN2,LOW);

digitalWrite(IN3,LOW);

digitalWrite(IN4,LOW);

}

void calF(){

digitalWrite(IN1,HIGH);

digitalWrite(IN2,LOW);

digitalWrite(IN3,LOW);

digitalWrite(IN4,HIGH);

delay(5000);

stopCar();

}

void calB(){

digitalWrite(IN1,LOW);

digitalWrite(IN2,HIGH);

digitalWrite(IN3,HIGH);

digitalWrite(IN4,LOW);

delay(5000);

stopCar();

}

void calR(){

digitalWrite(IN1,HIGH);

digitalWrite(IN2,LOW);

digitalWrite(IN3,HIGH);

digitalWrite(IN4,LOW);

delay(5000);

stopCar();

}

void calL(){

digitalWrite(IN1,LOW);

digitalWrite(IN2,HIGH);

digitalWrite(IN3,LOW);

digitalWrite(IN4,HIGH);

delay(5000);

stopCar();

}

Jetson Xavier NX

Jetson Xavier NX Lesson 11: Independently Tracking Different Objects in Different Cameras

July 29, 2020 admin

In this video lesson we show how two Raspberry Pi cameras can independently track to different objects of interest. As a demonstration, we track on two different colors, with pan/tilt servo systems adjusting to keep the object of interest in the center of the field of view.

In this project we are using the Jetson Xavier NX, which you can pick up HERE. You will also need to of the bracket/servo kits, which you can get HERE, and then two Raspberry Pi Version two cameras, available HERE.

import cv2
import numpy as np
import time
from adafruit_servokit import ServoKit
print(cv2.__version__)
timeMark=time.time()
dtFIL=0

def nothing(x):
    pass

cv2.namedWindow('TrackBars')
cv2.moveWindow('TrackBars',1320,0)
cv2.createTrackbar('hueLower', 'TrackBars',100,179,nothing)
cv2.createTrackbar('hueUpper', 'TrackBars',116,179,nothing)
cv2.createTrackbar('satLow', 'TrackBars',160,255,nothing)
cv2.createTrackbar('satHigh', 'TrackBars',255,255,nothing)
cv2.createTrackbar('valLow', 'TrackBars',150,255,nothing)
cv2.createTrackbar('valHigh', 'TrackBars',255,255,nothing)

cv2.namedWindow('TrackBars2')
cv2.moveWindow('TrackBars2',1100,0)
cv2.createTrackbar('hueLower2', 'TrackBars2',150,179,nothing)
cv2.createTrackbar('hueUpper2', 'TrackBars2',170,179,nothing)
cv2.createTrackbar('satLow2', 'TrackBars2',160,255,nothing)
cv2.createTrackbar('satHigh2', 'TrackBars2',255,255,nothing)
cv2.createTrackbar('valLow2', 'TrackBars2',150,255,nothing)
cv2.createTrackbar('valHigh2', 'TrackBars2',255,255,nothing)


kit=ServoKit(channels=16)

tilt1=90
pan1=90
tilt2=90
pan2=90

kit.servo[0].angle=pan1
kit.servo[1].angle=tilt1
kit.servo[2].angle=pan2
kit.servo[3].angle=tilt2

width=720
height=480
flip=2
font=cv2.FONT_HERSHEY_SIMPLEX
camSet1='nvarguscamerasrc sensor-id=0 ee-mode=1 ee-strength=0 tnr-mode=2 tnr-strength=1 wbmode=3 ! video/x-raw(memory:NVMM), width=3264, height=2464, framerate=21/1,format=NV12 ! nvvidconv flip-method='+str(flip)+' ! video/x-raw, width='+str(width)+', height='+str(height)+', format=BGRx ! videoconvert ! video/x-raw, format=BGR ! videobalance contrast=1.3 brightness=-.2 saturation=1.2 ! appsink drop=True'
camSet2='nvarguscamerasrc sensor-id=1 ee-mode=1 ee-strength=0 tnr-mode=2 tnr-strength=1 wbmode=3 ! video/x-raw(memory:NVMM), width=3264, height=2464, framerate=21/1,format=NV12 ! nvvidconv flip-method='+str(flip)+' ! video/x-raw, width='+str(width)+', height='+str(height)+', format=BGRx ! videoconvert ! video/x-raw, format=BGR ! videobalance contrast=1.3 brightness=-.2 saturation=1.2 ! appsink drop=True'

#camSet='nvarguscamerasrc sensor-id=0 ! video/x-raw(memory:NVMM), width=3264, height=2464, framerate=21/1,format=NV12 ! nvvidconv flip-method='+str(flip)+' ! video/x-raw, width='+str(width)+', height='+str(height)+', format=BGRx ! videoconvert ! video/x-raw, format=BGR ! appsink'
#camSet ='v4l2src device=/dev/video1 ! video/x-raw,width='+str(width)+',height='+str(height)+',framerate=20/1 ! videoconvert ! appsink'
cam1=cv2.VideoCapture(camSet1)
cam2=cv2.VideoCapture(camSet2)
while True:
    _, frame1 = cam1.read()
    _, frame2 = cam2.read()

    hsv1=cv2.cvtColor(frame1,cv2.COLOR_BGR2HSV)
    hsv2=cv2.cvtColor(frame2,cv2.COLOR_BGR2HSV)

    hueLow=cv2.getTrackbarPos('hueLower', 'TrackBars')
    hueUp=cv2.getTrackbarPos('hueUpper', 'TrackBars')

    Ls=cv2.getTrackbarPos('satLow', 'TrackBars')
    Us=cv2.getTrackbarPos('satHigh', 'TrackBars')

    Lv=cv2.getTrackbarPos('valLow', 'TrackBars')
    Uv=cv2.getTrackbarPos('valHigh', 'TrackBars')

    l_b=np.array([hueLow,Ls,Lv])
    u_b=np.array([hueUp,Us,Uv])


    hueLow2=cv2.getTrackbarPos('hueLower2', 'TrackBars2')
    hueUp2=cv2.getTrackbarPos('hueUpper2', 'TrackBars2')

    Ls2=cv2.getTrackbarPos('satLow2', 'TrackBars2')
    Us2=cv2.getTrackbarPos('satHigh2', 'TrackBars2')

    Lv2=cv2.getTrackbarPos('valLow2', 'TrackBars2')
    Uv2=cv2.getTrackbarPos('valHigh2', 'TrackBars2')

    l_b2=np.array([hueLow2,Ls2,Lv2])
    u_b2=np.array([hueUp2,Us2,Uv2])

    FGmask1=cv2.inRange(hsv1,l_b,u_b)
    FGmask2=cv2.inRange(hsv2,l_b2,u_b2)

    cv2.imshow('FGmask1',FGmask1)
    cv2.moveWindow('FGmask1',0,0)

    cv2.imshow('FGmask2',FGmask2)
    cv2.moveWindow('FGmask2',350,0)

    contours1,_ = cv2.findContours(FGmask1,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)
    contours1=sorted(contours1,key=lambda x:cv2.contourArea(x),reverse=True)
    for cnt in contours1:
        area=cv2.contourArea(cnt)
        (x,y,w,h)=cv2.boundingRect(cnt)
        if area>=100:
            cv2.rectangle(frame1,(x,y),(x+w,y+h),(0,255,255),3)
            objX=x+w/2
            objY=y+h/2
            errorPan1=objX-width/2
            errorTilt1=objY-height/2
            if abs(errorPan1)>15:
                pan1=pan1+errorPan1/40
            if abs(errorTilt1)>15:
                tilt1=tilt1-errorTilt1/40
            if pan1>180:
                pan1=180
                print('Pan Out of Range')
            if pan1<0:
                pan1=0
                print('Pan Out of Range')
            if tilt1>180:
                tilt1=180
                print('Tilt Out of Range')
            if tilt1<0:
                tilt1=0
            kit.servo[2].angle=pan1
            kit.servo[3].angle=tilt1
            break
    
    contours2,_ = cv2.findContours(FGmask2,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)
    contours2=sorted(contours2,key=lambda x:cv2.contourArea(x),reverse=True)
    for cnt in contours2:
        area=cv2.contourArea(cnt)
        (x,y,w,h)=cv2.boundingRect(cnt)
        if area>=100:
            cv2.rectangle(frame2,(x,y),(x+w,y+h),(0,255,255),3)
            objX=x+w/2
            objY=y+h/2
            errorPan2=objX-width/2
            errorTilt2=objY-height/2
            if abs(errorPan2)>15:
                pan2=pan2+errorPan2/40
            if abs(errorTilt2)>15:
                tilt2=tilt2-errorTilt2/40
            if pan2>180:
                pan2=180
                print('Pan Out of Range')
            if pan2<0:
                pan2=0
                print('Pan Out of Range')
            if tilt2>180:
                tilt2=180
                print('Tilt Out of Range')
            if tilt2<0:
                tilt2=0
            kit.servo[0].angle=pan2
            kit.servo[1].angle=tilt2            
            break

    frame3=np.hstack((frame1,frame2))
    dt=time.time()-timeMark
    timeMark=time.time()
    dtFIL=.9*dtFIL + .1*dt
    fps=1/dtFIL
    cv2.rectangle(frame3,(0,0),(150,40),(0,0,255),-1)
    cv2.putText(frame3,'fps: '+str(round(fps,1)),(0,30),font,1,(0,255,255),2)

    #cv2.imshow('myCam1',frame1)
    #cv2.imshow('myCam2',frame2)
    cv2.imshow('comboCam',frame3)
    cv2.moveWindow('comboCam',0,450)

    if cv2.waitKey(1)==ord('q'):
        break
cam1.release()
cam2.release()
cv2.destroyAllWindows()

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

import cv2

import numpy as np

import time

from adafruit_servokit import ServoKit

print(cv2.__version__)

timeMark=time.time()

dtFIL=0

def nothing(x):

pass

cv2.namedWindow('TrackBars')

cv2.moveWindow('TrackBars',1320,0)

cv2.createTrackbar('hueLower', 'TrackBars',100,179,nothing)

cv2.createTrackbar('hueUpper', 'TrackBars',116,179,nothing)

cv2.createTrackbar('satLow', 'TrackBars',160,255,nothing)

cv2.createTrackbar('satHigh', 'TrackBars',255,255,nothing)

cv2.createTrackbar('valLow', 'TrackBars',150,255,nothing)

cv2.createTrackbar('valHigh', 'TrackBars',255,255,nothing)

cv2.namedWindow('TrackBars2')

cv2.moveWindow('TrackBars2',1100,0)

cv2.createTrackbar('hueLower2', 'TrackBars2',150,179,nothing)

cv2.createTrackbar('hueUpper2', 'TrackBars2',170,179,nothing)

cv2.createTrackbar('satLow2', 'TrackBars2',160,255,nothing)

cv2.createTrackbar('satHigh2', 'TrackBars2',255,255,nothing)

cv2.createTrackbar('valLow2', 'TrackBars2',150,255,nothing)

cv2.createTrackbar('valHigh2', 'TrackBars2',255,255,nothing)

kit=ServoKit(channels=16)

tilt1=90

pan1=90

tilt2=90

pan2=90

kit.servo[0].angle=pan1

kit.servo[1].angle=tilt1

kit.servo[2].angle=pan2

kit.servo[3].angle=tilt2

width=720

height=480

flip=2

font=cv2.FONT_HERSHEY_SIMPLEX

#camSet ='v4l2src device=/dev/video1 ! video/x-raw,width='+str(width)+',height='+str(height)+',framerate=20/1 ! videoconvert ! appsink'

cam1=cv2.VideoCapture(camSet1)

cam2=cv2.VideoCapture(camSet2)

while True:

_, frame1 = cam1.read()

_, frame2 = cam2.read()

hsv1=cv2.cvtColor(frame1,cv2.COLOR_BGR2HSV)

hsv2=cv2.cvtColor(frame2,cv2.COLOR_BGR2HSV)

hueLow=cv2.getTrackbarPos('hueLower', 'TrackBars')

hueUp=cv2.getTrackbarPos('hueUpper', 'TrackBars')

Ls=cv2.getTrackbarPos('satLow', 'TrackBars')

Us=cv2.getTrackbarPos('satHigh', 'TrackBars')

Lv=cv2.getTrackbarPos('valLow', 'TrackBars')

Uv=cv2.getTrackbarPos('valHigh', 'TrackBars')

l_b=np.array([hueLow,Ls,Lv])

u_b=np.array([hueUp,Us,Uv])

hueLow2=cv2.getTrackbarPos('hueLower2', 'TrackBars2')

hueUp2=cv2.getTrackbarPos('hueUpper2', 'TrackBars2')

Ls2=cv2.getTrackbarPos('satLow2', 'TrackBars2')

Us2=cv2.getTrackbarPos('satHigh2', 'TrackBars2')

Lv2=cv2.getTrackbarPos('valLow2', 'TrackBars2')

Uv2=cv2.getTrackbarPos('valHigh2', 'TrackBars2')

l_b2=np.array([hueLow2,Ls2,Lv2])

u_b2=np.array([hueUp2,Us2,Uv2])

FGmask1=cv2.inRange(hsv1,l_b,u_b)

FGmask2=cv2.inRange(hsv2,l_b2,u_b2)

cv2.imshow('FGmask1',FGmask1)

cv2.moveWindow('FGmask1',0,0)

cv2.imshow('FGmask2',FGmask2)

cv2.moveWindow('FGmask2',350,0)

contours1,_ = cv2.findContours(FGmask1,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)

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

for cnt in contours1:

area=cv2.contourArea(cnt)

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

if area>=100:

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

objX=x+w/2

objY=y+h/2

errorPan1=objX-width/2

errorTilt1=objY-height/2

if abs(errorPan1)>15:

pan1=pan1+errorPan1/40

if abs(errorTilt1)>15:

tilt1=tilt1-errorTilt1/40

if pan1>180:

pan1=180

print('Pan Out of Range')

if pan1<0:

pan1=0

print('Pan Out of Range')

if tilt1>180:

tilt1=180

print('Tilt Out of Range')

if tilt1<0:

tilt1=0

kit.servo[2].angle=pan1

kit.servo[3].angle=tilt1

break

contours2,_ = cv2.findContours(FGmask2,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)

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

for cnt in contours2:

area=cv2.contourArea(cnt)

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

if area>=100:

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

objX=x+w/2

objY=y+h/2

errorPan2=objX-width/2

errorTilt2=objY-height/2

if abs(errorPan2)>15:

pan2=pan2+errorPan2/40

if abs(errorTilt2)>15:

tilt2=tilt2-errorTilt2/40

if pan2>180:

pan2=180

print('Pan Out of Range')

if pan2<0:

pan2=0

print('Pan Out of Range')

if tilt2>180:

tilt2=180

print('Tilt Out of Range')

if tilt2<0:

tilt2=0

kit.servo[0].angle=pan2

kit.servo[1].angle=tilt2

break

frame3=np.hstack((frame1,frame2))

dt=time.time()-timeMark

timeMark=time.time()

dtFIL=.9*dtFIL + .1*dt

fps=1/dtFIL

cv2.rectangle(frame3,(0,0),(150,40),(0,0,255),-1)

cv2.putText(frame3,'fps: '+str(round(fps,1)),(0,30),font,1,(0,255,255),2)

#cv2.imshow('myCam1',frame1)

#cv2.imshow('myCam2',frame2)

cv2.imshow('comboCam',frame3)

cv2.moveWindow('comboCam',0,450)

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

break

cam1.release()

cam2.release()

cv2.destroyAllWindows()

Technology Tutorials

All posts by admin

Robotics Training LESSON 9: Calibrate Robot Car Turn Angles with Linear Regression

Jetson Xavier NX Lesson 12: Intelligent Scanning for Objects of Interest

AI on the Jetson Nano LESSON 53: Object Detection and Recognition in OpenCV

Robotics Training LESSON 8: Setting Speed of the Smart Car

Jetson Xavier NX Lesson 11: Independently Tracking Different Objects in Different Cameras

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

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

Cookies