###### tags: 樹莓派,控制實驗
[TOC]
# 控制實驗-樹梅派
## [飆機器人官方網站](https://shop.playrobot.com/products/ros2-aiot-python-smart-bot)
## VNC網路線連線設定(Rpi)


## VNC網路線連線設定(PC)
1. 
1. 
1. 
1. 
1. 

### GPIO
[Raspberry Pi Pinout](https://pinout.xyz/pinout/pin18_gpio24)

## 安裝7-zip

## LED Button Control in python

import RPi.GPIO as gpio #引用RPi.GPIO函式庫
import time #引用time函式庫
LED_white = 14 #定義LED_white腳位
Buttun_0 = 15 #定義Buttun腳位
RED_LED_PIN = 2 #定義RED_LED腳位
GREEN_LED_PIN = 3 #定義GREEN_LED腳位
BLUE_LED_PIN = 4 #定義BLUE_LED腳位
PWM_FREQ = 10 #定義PWM_FREQ腳位

gpio.setwarnings(False) #關閉程式重複執行警告訊息
gpio.setmode(gpio.BCM)  #腳位定義為BCM
gpio.setup(LED_white, gpio.OUT) #定義為輸出腳位
gpio.setup(Buttun_0, gpio.IN)   #定義為輸入腳位
PWM = gpio.PWM(LED_white, 50)
PWM.start(0)

gpio.setmode(gpio.BCM)
gpio.setup(RED_LED_PIN, gpio.OUT)
gpio.setup(BLUE_LED_PIN, gpio.OUT)
gpio.setup(GREEN_LED_PIN, gpio.OUT)

red_pwm = gpio.PWM(RED_LED_PIN, PWM_FREQ)
red_pwm.start(0)
blue_pwm = gpio.PWM(BLUE_LED_PIN, PWM_FREQ)
blue_pwm.start(0)
green_pwm = gpio.PWM(GREEN_LED_PIN, PWM_FREQ)
green_pwm.start(0)

def setColor(r=0, g=0, b=0):
    red_pwm.ChangeDutyCycle(100-int(r/255*100))
    green_pwm.ChangeDutyCycle(100-int(g/255*100))
    blue_pwm.ChangeDutyCycle(100-int(b/255*100))

def ChangeLED():
    while True:
        dc = 0
        while(dc < 100):
            dc += 5
            PWM.ChangeDutyCycle(dc)
            time.sleep(0.5)
            print("PWM: " + str(dc))
            
def led_High_Low():
    gpio.output(LED_white, 1)
    time.sleep(1)
    gpio.output(LED_white, 0)
    time.sleep(1)
    print("End.")
    
def led_loop():
    while True:
        gpio.output(LED_white, 1)
        time.sleep(1)
        gpio.output(LED_white, 0)
        time.sleep(1)

def led_loop_count(count):
    i = 0
    while i < count:
        gpio.output(LED_white, 1)
        time.sleep(0.5)
        gpio.output(LED_white, 0)
        time.sleep(0.5)
        i += 1
        print("count: " + str(i))
        
def Btn_LED(hl):
    gpio.output(LED_white, hl)
    
def Btn_test():
    while True:
        Btn_LED(1-gpio.input(Buttun_0))

def Switch_LED(LED_num):
    if(LED_num == 1):
        #gpio.output(LED_white, 1)
        setColor(255, 0, 0)
    elif(LED_num == 2):
        #gpio.output(LED_white, 1)
        setColor(0, 255, 0)
    elif(LED_num == 3):
        #gpio.output(LED_white, 1)
        setColor(0, 0, 255)
        
def Btn_Switch():
    Switch_count = 1
    while True:
        btn_state = gpio.input(Buttun_0)
        if((not btn_state) and pre_btn_state):
            if(Switch_count > 3):
                Switch_count = 1
            print(Switch_count)
            Switch_LED(Switch_count)
            Switch_count += 1
        pre_btn_state = btn_state
        time.sleep(0.1)

if __name__ == "__main__":
    print("Start.")
    #led_High_Low()
    #led_loop_count(5)
    #Btn_test()
    #ChangeLED()
    Btn_Switch()

## LED Button Control in Arduino

const int Red = 9;
const int Green = 10;
const int Blue = 11;
const int LED_white = 12;
#define Btn_0 13

void setup() {
  Serial.begin(115200);
  pinMode(Red, OUTPUT);
  pinMode(Green, OUTPUT);
  pinMode(Blue, OUTPUT);
  pinMode(LED_white, OUTPUT);
  pinMode(Btn_0, INPUT);
}

void loop() {
  rgb_loop();
  led_High_Low(1000);
  led_loop_count(10, 1000);
  buttun_control_led();
}
void buttun_control_led()
{
  digitalWrite(LED_white, digitalRead(Btn_0));
}

void led_loop_count(int count, int delay_time)
{
#if 0
  int i = 0;
  while(i < count){
    digitalWrite(LED_white, 1);
    delay(delay_time);
    digitalWrite(LED_white, 0);
    delay(delay_time);
    i += 1;
    Serial.println("count: " + String(i));
  }
#endif
#if 1
  for(int i=0; i<count; i++){
    digitalWrite(LED_white, 1);
    delay(delay_time);
    digitalWrite(LED_white, 0);
    delay(delay_time);
    Serial.println("count: " + String(i));
  }
#endif
}

void led_High_Low(int delay_time)
{
    digitalWrite(LED_white, 1);
    delay(delay_time);  //延遲毫秒
    digitalWrite(LED_white, 0);
    delay(delay_time);
}
void rgb_loop()
{
  analogWrite(Red,255);
  analogWrite(Green,0);
  analogWrite(Blue,0);
  delay(1000);
  analogWrite(Red,0);
  analogWrite(Green,255);
  analogWrite(Blue,0);
  delay(1000);
  analogWrite(Red,0);
  analogWrite(Green,0);
  analogWrite(Blue,255);
  delay(1000);
}

## RGB Control in python

import RPi.GPIO as GPIO
import time
import random

RED_LED_PIN = 2
GREEN_LED_PIN = 3
BLUE_LED_PIN = 4
PWM_FREQ = 200

GPIO.setmode(GPIO.BCM)
GPIO.setup(RED_LED_PIN, GPIO.OUT)
GPIO.setup(BLUE_LED_PIN, GPIO.OUT)
GPIO.setup(GREEN_LED_PIN, GPIO.OUT)

red_pwm = GPIO.PWM(RED_LED_PIN, PWM_FREQ)
red_pwm.start(0)
blue_pwm = GPIO.PWM(BLUE_LED_PIN, PWM_FREQ)
blue_pwm.start(0)
green_pwm = GPIO.PWM(GREEN_LED_PIN, PWM_FREQ)
green_pwm.start(0)

def setColor(r=0, g=0, b=0):
    red_pwm.ChangeDutyCycle(100-int(r/255*100))
    green_pwm.ChangeDutyCycle(100-int(g/255*100))
    blue_pwm.ChangeDutyCycle(100-int(b/255*100))

try:
    print('按下 Ctrl-C 可停止程式')
    print('自動展示開始')
    print('紅色')
    setColor(255, 0, 0)
    time.sleep(1)
    print('綠色')
    setColor(0, 255, 0)
    time.sleep(1)
    print('藍色')
    setColor(0, 0, 255)
    time.sleep(1)
    print('熄滅')
    setColor(0, 0, 0)
    time.sleep(1)
    print('白色')
    setColor(255, 255, 255)
    time.sleep(1)
    print('自動展示結束')
    print('隨機顏色展示開始')
    while True:
        (r, g, b) = (random.randint(0, 255), random.randint(0, 255), random.randint(0, 255))
        print("#{:02X}{:02X}{:02X}".format(r, b, g))
        setColor(r, g, b)
        time.sleep(1)
except KeyboardInterrupt:
    print('關閉程式')
finally:
    red_pwm.stop()
    blue_pwm.stop()
    green_pwm.stop()
    GPIO.cleanup()

## RGB control in arduino

const int Red = 9;
const int Green = 10;
const int Blue = 11;


void setup() {
  pinMode(Red, OUTPUT);
  pinMode(Green, OUTPUT);
  pinMode(Blue, OUTPUT);
}

void loop() {
  analogWrite(Red,255);
  analogWrite(Green,0);
  analogWrite(Blue,0);
  delay(1000);
  analogWrite(Red,0);
  analogWrite(Green,255);
  analogWrite(Blue,0);
  delay(1000);
  analogWrite(Red,0);
  analogWrite(Green,0);
  analogWrite(Blue,255);
  delay(1000);
}

## Servo_Control in Arduino

#include <Servo.h>
#define RPi3 1
#define RPi4 0
#define Pin_Motor_Left 12
#define Pin_Motor_Right 13

Servo Motor_Left;
Servo Motor_Right;
#if RPi3
const int trig_1 = 10;
const int trig_2 = 11;
#endif
#if RPi4
const int trig_1 = 8;
const int echo_1 = 9;
const int trig_2 = 10;
const int echo_2 = 11;
#endif
void setup() {
  Serial.begin(115200);
  Motor_Left.attach(Pin_Motor_Left);
  Motor_Right.attach(Pin_Motor_Right);
  #if RPi4
  pinMode(trig_1, OUTPUT);
  pinMode(echo_1, INPUT);
  pinMode(trig_2, OUTPUT);
  pinMode(echo_2, INPUT);
  #endif
}
void loop() {
  #if RPi3
  Serial.print(ReadDistance(trig_1));
  Serial.print(",");
  Serial.println(ReadDistance(trig_2));
  #endif
  #if RPi4
  Serial.print(ReadDistance(trig_1, echo_1));
  Serial.print(",");
  Serial.println(ReadDistance(trig_1, echo_1));
  #endif
  //delay(500);
  switch(ReciveMsg())
  {
    case 'X':
      ServoControl(1600, 1400);
      break;
    case 'A':
      ServoControl(1400, 1400);
      break;
    case 'W':
      ServoControl(1400, 1600);
      break;
    case 'D':
      ServoControl(1600, 1600);
      break;
    case 'S':
      ServoControl(1500, 1500);
      break;  
  }
  
}

char ReciveMsg()
{
  char msg;
  if(Serial.available())
  {
    msg = Serial.read();
    return msg;
  }
}

void ServoControl(int speed_L, int speed_R)
{
  Motor_Left.writeMicroseconds(speed_L);
  Motor_Right.writeMicroseconds(speed_R);
}
long ReadDistance(int trig_pin) //For Rpi3
{
  pinMode(trig_pin, OUTPUT);
  long distance = 0;
  digitalWrite(trig_pin, LOW);
  delayMicroseconds(2);
  digitalWrite(trig_pin, HIGH);
  delayMicroseconds(10);
  pinMode(trig_pin, INPUT);
  distance = pulseIn(trig_pin, HIGH)/2/29;
  return distance;
}
long ReadDistance(int trig_pin, int echo_pin) //For Rpi4
{
  long distance = 0;
  digitalWrite(trig_pin, LOW);
  delayMicroseconds(2);
  digitalWrite(trig_pin, HIGH);
  delayMicroseconds(10);
  distance = pulseIn(echo_pin, HIGH)/2/29;
  return distance;
}

## Servo speed control in Arduino

#include <Servo.h>
#define RPi3 1
#define RPi4 0
#define CONTROL_SPEED 0
#define Pin_Motor_Left 12
#define Pin_Motor_Right 13

Servo Motor_Left;
Servo Motor_Right;
#if RPi3
const int trig_1 = 10;
const int trig_2 = 11;
#endif
#if RPi4
const int trig_1 = 8;
const int echo_1 = 9;
const int trig_2 = 10;
const int echo_2 = 11;
#endif
char receive_char;

const uint8_t analogPin = A0;
int resistance_value;
int servo_speed_Forward;
int servo_speed_Reverse;
int workindex = -1;
int preworkindex = -1;
long last_time;
void setup() {
  Serial.begin(115200);
  Motor_Left.attach(Pin_Motor_Left);
  Motor_Right.attach(Pin_Motor_Right);
#if RPi4
  pinMode(trig_1, OUTPUT);
  pinMode(echo_1, INPUT);
  pinMode(trig_2, OUTPUT);
  pinMode(echo_2, INPUT);
#endif
}
void loop() {
  loop_run(1000, 3000);
#if RPi3
  Serial.print(ReadDistance(trig_1));
  Serial.print(",");
  Serial.println(ReadDistance(trig_2));
#endif

#if RPi4
  Serial.print(ReadDistance(trig_1, echo_1));
  Serial.print(",");
  Serial.println(ReadDistance(trig_1, echo_1));
#endif
  delay(100);
#if 0
  switch(ReciveMsg())
  {
    case 'X':
      ServoControl(1600, 1400);
      break;
    case 'A':
      ServoControl(1400, 1400);
      break;
    case 'W':
      ServoControl(1400, 1600);
      break;
    case 'D':
      ServoControl(1600, 1600);
      break;
    case 'S':
      ServoControl(1500, 1500);
      break;  
  }
#endif
#if CONTROL_SPEED
  resistance_value = analogRead(analogPin);
//  Serial.println(resistance_value);
  servo_speed_Forward = map(resistance_value, 0, 255, 1500, 1700);
  servo_speed_Reverse = map(resistance_value, 0, 255, 1500, 1300);
//  Serial.print(servo_speed_Forward);
//  Serial.print(",");
//  Serial.println(servo_speed_Reverse);
  if(Serial.available())
  {
    receive_char = Serial.read();
  }
  switch(receive_char)
  {
    case 'X':
      ServoControl(servo_speed_Forward, servo_speed_Reverse);
      break;
    case 'A':
      ServoControl(servo_speed_Reverse, servo_speed_Reverse);
      break;
    case 'W':
      ServoControl(servo_speed_Reverse, servo_speed_Forward);
      break;
    case 'D':
      ServoControl(servo_speed_Forward, servo_speed_Forward);
      break;
    case 'S':
      ServoControl(1500, 1500);
      break;  
  }
#endif
}

void loop_run(long time1, long time2)
{
  if(preworkindex != workindex){
    preworkindex = workindex;
    Serial.println("workindex: " + String(preworkindex));
  }
  switch(workindex)
  {
    case 0:
      if(millis() - last_time > time1){
        ServoControl(1400, 1600); //foward
        workindex += 10;
        last_time = millis();
      }
      break;
    case 10:
      if(millis() - last_time > time2){
        ServoControl(1600, 1600);//Right
        workindex += 10;
        last_time = millis();
      }
      break;
    case 20:
      if(millis() - last_time > time1){
        ServoControl(1400, 1600); //foward
        workindex += 10;
        last_time = millis();
      }
      break;
    case 30:
      if(millis() - last_time > time2){
        ServoControl(1600, 1600);//Right
        workindex = 0;
        last_time = millis();
      }
      break;
  }
}

char ReciveMsg()
{
  char msg;
  if(Serial.available())
  {
    msg = Serial.read();
    return msg;
  }
}

void ServoControl(int speed_L, int speed_R)
{
  Motor_Left.writeMicroseconds(speed_L);
  Motor_Right.writeMicroseconds(speed_R);
}
long ReadDistance(int trig_pin) //For Rpi3
{
  pinMode(trig_pin, OUTPUT);
  long distance = 0;
  digitalWrite(trig_pin, LOW);
  delayMicroseconds(2);
  digitalWrite(trig_pin, HIGH);
  delayMicroseconds(10);
  pinMode(trig_pin, INPUT);
  distance = pulseIn(trig_pin, HIGH)/2/29;
  return distance;
}
long ReadDistance(int trig_pin, int echo_pin) //For Rpi4
{
  long distance = 0;
  digitalWrite(trig_pin, LOW);
  delayMicroseconds(2);
  digitalWrite(trig_pin, HIGH);
  delayMicroseconds(10);
  distance = pulseIn(echo_pin, HIGH)/2/29;
  return distance;
}

[手把手教你看懂并理解Arduino PID控制库](https://my.oschina.net/u/3162997/blog/809733)

## 樹梅派 相機 拍照
[ord函數](https://www.runoob.com/python/python-func-ord.html)
[OpenCV 擷取網路攝影機串流影像，處理並寫入影片檔案教學](https://blog.gtwang.org/programming/opencv-webcam-video-capture-and-file-write-tutorial/)

#!/usr/bin/env python
# -*- coding: utf8 -*-
import cv2
print("cv2 version: " + cv2.__version__)
import RPi.GPIO as GPIO
import time
import numpy as np

GPIO.setmode(GPIO.BCM)
GPIO.setup(17,GPIO.IN)

cap = cv2.VideoCapture(0)
ret, frame = cap.read()
print(cap.get(3), cap.get(4))
picture_num = 0
pre_btn_state = 1
while(True):
    ret, hframe = cap.read()
    cv2.imshow('image', hframe)
    if GPIO.input(17) == 0 and (not pre_btn_state):
        time.sleep(0.3)
        print('photograph')
        cv2.imwrite('Output.jpg', hframe) // 輸出圖片(存檔路徑為目前.py路徑底下)
        ''' //
        picture_num = picture_num + 1
        print('photograph')
        path = '/home/pi/Desktop/Control_exp_v_2/' //設定存檔絕對路徑
        cv2.imwrite(path + 'output_' + str(picture_num) + '.jpg', hframe) //
        '''
    pre_btn_state = GPIO.input(17)
    if cv2.waitKey(1) & 0xFF == ord('q'):
        cap.release()
        cv2.destroyAllWindows()
        break

## 影像圖片放大縮小

#!/usr/bin/env python
# -*- coding: utf8 -*-
import cv2
print("cv2 version: " + cv2.__version__)
import numpy as np

img = cv2.imread('123.jpg')
print(img.shape)
rows, cols, channel = img.shape

dst = cv2.resize(img, (2*cols, rows), interpolation = cv2.INTER_CUBIC)
dst1 = cv2.resize(img, (cols, 2*rows), interpolation = cv2.INTER_LINEAR)
dst2 = cv2.resize(img, (int(cols/2), int(rows/2)), interpolation = cv2.INTER_AREA)

cv2.imshow('Original', img)
cv2.imshow('INTER_CUBIC', dst)
cv2.imshow('INTER_LINEAR', dst1)
cv2.imshow('INTER_AREA', dst2)
cv2.waitKey(0)
cv2.destroyAllWindows()

## 圖片二值化

#!/usr/bin/env python
# -*- coding: utf8 -*-
import cv2
print("cv2 version: " + cv2.__version__)
import numpy as np
import matplotlib
matplotlib.use('Tkagg', warn = False, force = True)
from matplotlib import pyplot as plt

img = cv2.imread('456.jpg')

ret, thresh1 = cv2.threshold(img, 127, 155, cv2.THRESH_BINARY)
ret, thresh2 = cv2.threshold(img, 127, 255, cv2.THRESH_BINARY_INV)
ret, thresh3 = cv2.threshold(img, 127, 5, cv2.THRESH_TRUNC)
ret, thresh4 = cv2.threshold(img, 127, 255, cv2.THRESH_TOZERO)
ret, thresh5 = cv2.threshold(img, 127, 255, cv2.THRESH_TOZERO_INV)

titles = ['Original', 'BINARY', 'BINARY_INV', 'TRUNC', 'TOZERO', 'TOZERO_INV']
images = [img, thresh1, thresh2, thresh3, thresh4, thresh5]

for i in range:
    plt.subplot(2,3,i+1), plt.imshow(images[i])
    plt.title(titles[i])
    plt.xticks([]),plt.yticks([])
plt.show()

## 超音波控制

#define RPi3 1
#define RPi4 0
#if RPi3
const int trig_1 = 10;
const int trig_2 = 11;
#endif
#if RPi4
const int trig_1 = 8;
const int echo_1 = 9;
const int trig_2 = 10;
const int echo_2 = 11;
#endif
void setup() {
  Serial.begin(115200);
#if RPi4
  pinMode(trig_1, OUTPUT);
  pinMode(echo_1, INPUT);
  pinMode(trig_2, OUTPUT);
  pinMode(echo_2, INPUT);
#endif
}
void loop(){
#if RPi3
  Serial.print(ReadDistance(trig_1));
  Serial.print(",");
  Serial.println(ReadDistance(trig_2));
#endif
#if RPi4
  Serial.print(ReadDistance(trig_1, echo_1));
  Serial.print(",");
  Serial.println(ReadDistance(trig_1, echo_1));
#endif
}
long ReadDistance(int trig_pin) //For Rpi3
{
  pinMode(trig_pin, OUTPUT);
  long distance = 0;
  digitalWrite(trig_pin, LOW);
  delayMicroseconds(2);
  digitalWrite(trig_pin, HIGH);
  delayMicroseconds(10);
  pinMode(trig_pin, INPUT);
  distance = pulseIn(trig_pin, HIGH)/2/29;
  return distance;
}
long ReadDistance(int trig_pin, int echo_pin) //For Rpi4
{
  long distance = 0;
  digitalWrite(trig_pin, LOW);
  delayMicroseconds(2);
  digitalWrite(trig_pin, HIGH);
  delayMicroseconds(10);
  distance = pulseIn(echo_pin, HIGH)/2/29;
  return distance;
}

## PID控制

#include <Servo.h>         //載入函式庫
#define Pin_Motor_Left 12  //定義左邊馬達腳位
#define Pin_Motor_Right 13 //定義右邊馬達腳位
#define RPi3 0             //確認是否為正確的樹梅派版本
#define RPi4 1
#define target_distance 30 //停止目標距離

#if RPi3
const int trig_1 = 10;
const int trig_2 = 11;
#endif
#if RPi4
const int trig_1 = 8;
const int echo_1 = 9;
const int trig_2 = 10;
const int echo_2 = 11;
#endif
  
Servo Motor_Left;       // 建立SERVO物件
Servo Motor_Right;      // 建立SERVO物件

int interror = 0;
int olderror = 0;
double values;
long pre_times = 0;

double Kp = 10, Ki = 0.03, Kd = 1;

void setup() {
  Serial.begin(115200);//設定通訊鮑率

  // 設定要將伺服馬達接到哪一個PIN腳
  Motor_Left.attach(Pin_Motor_Left);    
  Motor_Right.attach(Pin_Motor_Right);
  Serial.begin(115200);
  #if RPi4
    pinMode(trig_1, OUTPUT);
    pinMode(echo_1, INPUT);
    pinMode(trig_2, OUTPUT);
    pinMode(echo_2, INPUT);
  #endif  
}

void loop() {
  if(Serial.available() > 0)
    {
      char c = Serial.read();
      Serial.println("receive_char: " + String(c));
      if(c == 'S')
      {
        ServoControl(1500, 1500);
        Serial.println("Stop");
        while(1){}
      }
    }
    if(millis() - pre_times > 500){
        #if RPi3
            values = ReadDistance(trig_1);
        #endif
        #if RPi4
            values = ReadDistance(trig_1, echo_1);
        #endif
        if(values != 0)
            PID_run();
        pre_times = millis();
    }
    //delay(100);
}

void PID_run()
{
    int error = (target_distance - (int)values);
    interror += error;
    int lasterror = error - olderror;
    olderror = error; 
    int pid_value = error * Kp + interror * Ki + lasterror * Kd;
    /* error(P值)、interror(I值)、lasterror(D值)、olderror用來取D值的 */
    Serial.println(values);
    
    //限制PID輸出值:
    if (pid_value > 200)  //  1300 ~ 1700
      pid_value = 200;
    if (pid_value < -200)
      pid_value = -200;

    if (error > 0) //判斷太遠或太近
      ServoControl(1500 + pid_value, 1500 - pid_value); //太近 後退
    else if(error < 0)
      ServoControl(1500 + pid_value, 1500 - pid_value); //太遠 前進
    else 
      ServoControl(1500, 1500); //停止
}


long ReadDistance(int trig_pin) //For Rpi3
{
  pinMode(trig_pin, OUTPUT);
  long distance = 0;
  digitalWrite(trig_pin, LOW);
  delayMicroseconds(2);
  digitalWrite(trig_pin, HIGH);
  delayMicroseconds(10);
  pinMode(trig_pin, INPUT);
  distance = pulseIn(trig_pin, HIGH)/2/29;
  return distance;
}
long ReadDistance(int trig_pin, int echo_pin) //For Rpi4
{
  long distance = 0;
  digitalWrite(trig_pin, LOW);
  delayMicroseconds(2);
  digitalWrite(trig_pin, HIGH);
  delayMicroseconds(10);
  distance = pulseIn(echo_pin, HIGH)/2/29;
  return distance;
}

void ServoControl(int speed_L, int speed_R)
{
  Motor_Left.writeMicroseconds(speed_L);
  Motor_Right.writeMicroseconds(speed_R);
}