stm32设置
void M1_PID(float top);
void upmada(void);
typedef struct
{
struct
{
uint16_t ch0;
uint16_t ch1;
uint16_t ch2;
uint16_t ch3;
uint8_t s1;
uint8_t s2;
}rc;
struct {
int16_t x;
int16_t y;
int16_t z;
uint8_t press_l;
uint8_t press_r;
}mouse;
struct {
uint16_t v;
}key;
}
RC_Ctl_t;
static RC_Ctl_t RC_CtrlData;
typedef struct {
float p;
float i;
float d;
float e0;
float e1;
float e2;
float zi;
}PID;
static PID M1_pid;
uint8_t aTxStartMessages[] = "1203";
volatile uint8_t aRxBuffer[30];
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
UNUSED(GPIO_Pin);
if(GPIO_Pin == GPIO_PIN_8){
HAL_GPIO_TogglePin(GPIOF,GPIO_PIN_9);
}
}
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
if(htim->Instance == TIM10){
upmada();
}
}
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
UNUSED(huart);
switch(aRxBuffer[0]){
case 'a' :
RC_CtrlData.rc.ch0 = (uint16_t)(aRxBuffer[1]-'0')*100+(uint16_t)(aRxBuffer[2]-'0')*10+(uint16_t)(aRxBuffer[3]-'0'); printf("%d",RC_CtrlData.rc.ch0);
RC_CtrlData.rc.ch1 = (uint16_t)(aRxBuffer[4]-'0')*100+(uint16_t)(aRxBuffer[5]-'0')*10+(uint16_t)(aRxBuffer[6]-'0'); printf("%d",RC_CtrlData.rc.ch1);
RC_CtrlData.rc.ch2 = (uint16_t)(aRxBuffer[7]-'0')*100+(uint16_t)(aRxBuffer[8]-'0')*10+(uint16_t)(aRxBuffer[9]-'0'); printf("%d",RC_CtrlData.rc.ch2);
RC_CtrlData.rc.ch3 = (uint16_t)(aRxBuffer[10]-'0')*100+(uint16_t)(aRxBuffer[11]-'0')*10+(uint16_t)(aRxBuffer[12]-'0'); printf("%d",RC_CtrlData.rc.ch3);
RC_CtrlData.rc.s1 = (uint16_t)(aRxBuffer[13]-'0')*100+(uint16_t)(aRxBuffer[14]-'0')*10+(uint16_t)(aRxBuffer[15]-'0'); printf("%d",RC_CtrlData.rc.s1);
RC_CtrlData.rc.s2 = (uint16_t)(aRxBuffer[16]-'0')*100+(uint16_t)(aRxBuffer[17]-'0')*10+(uint16_t)(aRxBuffer[18]-'0'); printf("%d",RC_CtrlData.rc.s2); printf("ok");break ;
default: printf(" no word **");
HAL_UART_DMAStop(&huart1);
HAL_UART_Receive_DMA(&huart1,(uint8_t*)aRxBuffer,20);
;
}
}
void upmada(void){
if(M1_pid.zi>500){
__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_2,0);
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_1,M1_pid.zi-500);
}
else if(M1_pid.zi < 500){
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_1,0);
__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_2,500-M1_pid.zi);
}
else if(M1_pid.zi == 500){
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_1,0);
__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_2,0);
}
M1_PID(RC_CtrlData.rc.ch0);
}
void M1_PID(float top){
float put;
M1_pid.e0 = (top - M1_pid.zi);
put = M1_pid.p*(M1_pid.e0-M1_pid.e1)+M1_pid.i*M1_pid.e0+M1_pid.d*(M1_pid.e0-2*M1_pid.e2+M1_pid.e2);
M1_pid.e2 = M1_pid.e1;
M1_pid.e1 = M1_pid.e0;
M1_pid.zi += put;
}
M1_pid.p = 0.1;
M1_pid.i = 0.1;
M1_pid.d = 0.007;
M1_pid.e0 = 0;
M1_pid.e1 = 0;
M1_pid.e2 = 0;
M1_pid.zi = 500;
RC_CtrlData.rc.ch0 = 500;
__HAL_TIM_CLEAR_IT(&htim10,TIM_IT_UPDATE);
HAL_TIM_Base_Start_IT(&htim10);
HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_1);
HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_2);
__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_2,0);
HAL_UART_Transmit_IT(&huart1 ,(uint8_t*)aTxStartMessages,sizeof(aTxStartMessages));
HAL_UART_Receive_DMA(&huart1,(uint8_t*)aRxBuffer,20);
树莓派代码
import serial
import time
import RPi.GPIO as gpio
import cv2
import numpy as np
import threading
from threading import Lock,Thread
class USARTOR:
def __init__(self):
self.com = "/dev/ttyUSB0"
self.btl = 9600
self.ts = 'a000123123123123123'
self.rs = 'a000123123123123123'
def setup(self):
self.ser = serial.Serial(self.com ,self.btl)
if self.ser.isOpen == False:
self.ser.open()
def Writer(self):
self.ser.write(self.ts.encode())
self.ser.flushInput()
Usartor = USARTOR()
Usartor.setup()
def get3(num):
num = (int)(num)
Num = str((int(num/100))) + str((int((num/10)%10)) ) + str(int(num%10))
return str(Num)
def callback(object):
pass
def update():
img = np.zeros((200, 400, 3), np.uint8)
cv2.namedWindow('image')
cv2.createTrackbar('R', 'image', 0, 999, callback)
while True:
theta0 = cv2.getTrackbarPos('R', 'image')
theta0 = get3(theta0)
Usartor.ts = Usartor.ts[0]+ theta0 + Usartor.ts[4:]
Usartor.Writer()
if cv2.waitKey(4) & 0xFF == ord('q'):
break
cv2.imshow('image', img)
cv2.destroyAllWindows()
def main():
update()
if __name__ == '__main__':
main()
主要思想:树莓派与电脑客户端(pyqt5制作)通过socket局域网通信(还没有做),树莓派与32通过USB端口进行串口通信(双向),32控制马达转动(PID,PWM,DMA)。 未完待续…
|