cv2.putText(img, " P I D", (400, 45), cv2.FONT_HERSHEY_DUPLEX, 0.4, (0, 255, 0))

"+str(xD),

127, 127))

"+str(yD),

127, 127))

"+str(zD),

cv2.putText(img, " "+str(xP)+"

(370, 60), cv2.FONT_HERSHEY_DUPLEX,

cv2.putText(img, " "+str(yP)+"

(370, 75), cv2.FONT_HERSHEY_DUPLEX,

cv2.putText(img, " "+str(zP)+"

"+str(xI)+" 0.4, (255, "+str(yI)+" 0.4, (255, "+str(zI)+"

 

(370, 90), cv2.FONT_HERSHEY_DUPLEX, 0.4, (255,

## estimate roll

if (c_x > 350 or c_x < 290 ) and active:

s_roll = ctrl_roll.update(c_x)

# non acummulative output

ut_roll = 1500 + translate(s_roll, -200, 200, -100, 100) #ut_roll = ut_roll + translate(s_roll, -200, 200, -20, 20) ut_roll = ut_roll if ut_roll < maxPWM else maxPWM

ut_roll = ut_roll if ut_roll > minPWM else minPWM

cv2.putText(img, "[Roll] :: "+str(int(ut_roll)), (10, 60),

cv2.FONT_HERSHEY_DUPLEX, 0.4, (0, 255, 255)) pi0.set_servo_pulsewidth(pin_roll, ut_roll)

else:

cv2.putText(img, "[Roll] :: OK", (10, 60), cv2.FONT_HERSHEY_DUPLEX, 0.4, (0, 255, 0))

20)

pi0.set_servo_pulsewidth(pin_roll, 0)

 

 

 

 

## estimate pitch

if (c_y > 270 or c_y < 210 ) and active:

s_pitch = ctrl_pitch.update(c_y)

# non acummulative output

ut_pitch = 1500 + translate(s_pitch, -200, 200, -70, 70) # ut_pitch = ut_pitch + translate(s_pitch, -200, 200, -20,

ut_pitch = ut_pitch if ut_pitch < maxPWM else maxPWM ut_pitch = ut_pitch if ut_pitch > minPWM else minPWM cv2.putText(img, "[Pitch] :: "+str(int(ut_pitch)), (10,

75), cv2.FONT_HERSHEY_DUPLEX, 0.4, (0, 255, 255)) pi0.set_servo_pulsewidth(pin_pitch, ut_pitch)