PID tuned for 2S battery

Code/STM32/Core/Src/Components/BTS7960B.cpp

@@ -7,23 +7,29 @@
 
 #include "BTS7960B.hpp"
 
-BTS7960B::BTS7960B(__IO uint32_t* ina_ccr, __IO uint32_t* inb_ccr, GPIO_TypeDef* inha_gpio_port, uint16_t inha_gpio_pin, GPIO_TypeDef* inhb_gpio_port, uint16_t inhb_gpio_pin)
- : ina(ina_ccr), inb(inb_ccr), inha_port(inha_gpio_port), inha_pin(inha_gpio_pin), inhb_port(inhb_gpio_port), inhb_pin(inhb_gpio_pin){
+BTS7960B::BTS7960B(__IO uint32_t* ina_ccr, __IO uint32_t* inb_ccr, __IO uint32_t* ina_arr, __IO uint32_t* inb_arr, GPIO_TypeDef* inha_gpio_port, uint16_t inha_gpio_pin, GPIO_TypeDef* inhb_gpio_port, uint16_t inhb_gpio_pin)
+ : ina(ina_ccr), inb(inb_ccr), ina_max(ina_arr), inb_max(inb_arr), inha_port(inha_gpio_port), inha_pin(inha_gpio_pin), inhb_port(inhb_gpio_port), inhb_pin(inhb_gpio_pin){
 
 }
 
-void BTS7960B::setSpeed(int32_t speed){
-
-	if(speed > 0){
+void BTS7960B::setPower(float power){
+	if(power >= 0 && power <= 1){
 		HAL_GPIO_WritePin(inha_port, inha_pin, GPIO_PIN_SET);
 		HAL_GPIO_WritePin(inhb_port, inhb_pin, GPIO_PIN_SET);
-		*ina = speed;
+		*ina = power*(*ina_max);
 		*inb = 0;
-	}else if(speed < 0){
+	}else if(power < 0 && power >= -1){
 		HAL_GPIO_WritePin(inha_port, inha_pin, GPIO_PIN_SET);
 		HAL_GPIO_WritePin(inhb_port, inhb_pin, GPIO_PIN_SET);
 		*ina = 0;
-		*inb = -speed;
+		*inb = -power*(*inb_max);
+	}
+}
+
+void BTS7960B::setEnabled(bool enabled){
+	if(enabled){
+		HAL_GPIO_WritePin(inha_port, inha_pin, GPIO_PIN_SET);
+		HAL_GPIO_WritePin(inhb_port, inhb_pin, GPIO_PIN_SET);
 	}else{
 		HAL_GPIO_WritePin(inha_port, inha_pin, GPIO_PIN_RESET);
 		HAL_GPIO_WritePin(inhb_port, inhb_pin, GPIO_PIN_RESET);

Code/STM32/Core/Src/Components/BTS7960B.hpp

@@ -9,14 +9,18 @@
 #define SRC_COMPONENTS_BTS7960B_HPP_
 
 #include "main.h"
+#include "Motor.hpp"
 
-class BTS7960B {
+class BTS7960B : public Motor{
 public:
-	BTS7960B(__IO uint32_t* ina_ccr, __IO uint32_t* inb_ccr, GPIO_TypeDef* inha_gpio_port, uint16_t inha_gpio_pin, GPIO_TypeDef* inhb_gpio_port, uint16_t inhb_gpio_pin);
-	void setSpeed(int32_t speed);
+	BTS7960B(__IO uint32_t* ina_ccr, __IO uint32_t* inb_ccr, __IO uint32_t* ina_arr, __IO uint32_t* inb_arr, GPIO_TypeDef* inha_gpio_port, uint16_t inha_gpio_pin, GPIO_TypeDef* inhb_gpio_port, uint16_t inhb_gpio_pin);
+	void setPower(float power);
+	void setEnabled(bool enabled);
 private:
 	__IO uint32_t* ina;
 	__IO uint32_t* inb;
+	__IO uint32_t* ina_max;
+	__IO uint32_t* inb_max;
 	GPIO_TypeDef* inha_port;
 	uint16_t inha_pin;
 	GPIO_TypeDef* inhb_port;

Code/STM32/Core/Src/Components/Encoder.cpp

@@ -0,0 +1,33 @@
+/*
+ * Encoder.cpp
+ *
+ *  Created on: Mar 17, 2023
+ *      Author: Gabriel
+ */
+
+#include "Encoder.hpp"
+
+Encoder::Encoder(__IO uint32_t* cnt, __IO uint32_t* arr, uint32_t countsPerRevolution) :
+	cnt(cnt), arr(arr), countsPerRevolution(countsPerRevolution)
+{
+
+}
+
+Encoder::~Encoder() {
+
+}
+
+uint32_t Encoder::getCount(){
+	lastCount = *cnt;
+	return lastCount;
+}
+
+int16_t Encoder::getDelta(){
+	int16_t delta =  *cnt - lastCount;
+	lastCount = *cnt;
+	return delta;
+}
+
+uint32_t Encoder::getCountsPerRevolution(){
+	return countsPerRevolution;
+}

Code/STM32/Core/Src/Components/Encoder.hpp

@@ -0,0 +1,27 @@
+/*
+ * Encoder.hpp
+ *
+ *  Created on: Mar 17, 2023
+ *      Author: Gabriel
+ */
+
+#ifndef SRC_COMPONENTS_ENCODER_HPP_
+#define SRC_COMPONENTS_ENCODER_HPP_
+
+#include "main.h"
+
+class Encoder {
+public:
+	Encoder(__IO uint32_t* cnt, __IO uint32_t* arr, uint32_t countsPerRevolution);
+	virtual ~Encoder();
+	uint32_t getCount();
+	int16_t getDelta();
+	uint32_t getCountsPerRevolution();
+private:
+	__IO uint32_t* cnt;
+	__IO uint32_t* arr;
+	uint32_t lastCount = 0;
+	uint32_t countsPerRevolution = 0;
+};
+
+#endif /* SRC_COMPONENTS_ENCODER_HPP_ */

Code/STM32/Core/Src/Components/Motor.cpp

@@ -0,0 +1,45 @@
+/*
+ * Motor.cpp
+ *
+ *  Created on: Mar 17, 2023
+ *      Author: Gabriel
+ */
+
+#include "Motor.hpp"
+#include <cstdio>
+
+Motor::Motor() {
+
+}
+
+Motor::~Motor() {
+
+}
+
+void Motor::setEncoder(Encoder* _encoder){
+	encoder = _encoder;
+}
+
+float Motor::getCurrentRevPerTick(){
+	return currentRevPerTick;
+}
+
+void Motor::pid(float inputRevPerTick){
+	currentRevPerTick = (float)encoder->getDelta()/encoder->getCountsPerRevolution();
+	lastError = error;
+	error = inputRevPerTick - currentRevPerTick;
+	derror = error - lastError;
+	ierror += error;
+	if(ierror > 1){
+		ierror = 1;
+	}else if(ierror < -1){
+		ierror = -1;
+	}
+	float pid = kp*error + ki*ierror + kd*derror;
+	if(pid > 1){
+		pid = 1;
+	}else if(pid<-1){
+		pid = -1;
+	}
+	setPower(pid);
+}

Code/STM32/Core/Src/Components/Motor.hpp

@@ -0,0 +1,34 @@
+/*
+ * Motor.hpp
+ *
+ *  Created on: Mar 17, 2023
+ *      Author: Gabriel
+ */
+
+#ifndef SRC_COMPONENTS_MOTOR_HPP_
+#define SRC_COMPONENTS_MOTOR_HPP_
+
+#include "main.h"
+#include "Encoder.hpp"
+
+class Motor {
+public:
+	Motor();
+	virtual ~Motor();
+	virtual void setPower(float power){}
+	void setEncoder(Encoder* _encoder);
+	float getCurrentRevPerTick();
+	void pid(float inputRevPerTick);
+private:
+	Encoder* encoder;
+	float kp = 1.9852;
+	float ki = 53.0902*0.01;
+	float kd = 0/0.01;
+	float error = 0;
+	float ierror = 0;
+	float derror = 0;
+	float lastError = 0;
+	float currentRevPerTick = 0;
+};
+
+#endif /* SRC_COMPONENTS_MOTOR_HPP_ */

Code/STM32/Core/Src/Components/Robot.cpp

@@ -0,0 +1,68 @@
+/*
+ * Robot.cpp
+ *
+ *  Created on: Mar 17, 2023
+ *      Author: Gabriel
+ */
+
+#include "Robot.hpp"
+#include <cstdlib>
+#include "SerialDebug.hpp"
+
+#define CONVERSION (reductionRatio/(ticksPerSecond*2*3.14159*wheelRadius))
+
+extern UART_HandleTypeDef huart1;
+extern TIM_HandleTypeDef htim1;
+extern TIM_HandleTypeDef htim2;
+extern TIM_HandleTypeDef htim3;
+extern TIM_HandleTypeDef htim4;
+extern SerialDebug debug;
+
+uint8_t buf[64];
+
+Robot::Robot() {
+
+}
+
+Robot::~Robot() {
+
+}
+
+void Robot::uartCallback(){
+	HAL_UARTEx_ReceiveToIdle_DMA(&huart1, rxBuffer, 256);
+}
+
+void Robot::controlCallback(){
+	HAL_GPIO_TogglePin(LED_BUILTIN_GPIO_Port, LED_BUILTIN_Pin);
+	//float desiredSpeed0 = strtof((const char*)rxBuffer, NULL);
+	float desiredSpeed0;
+	float desiredSpeed1;
+	sscanf((char*)rxBuffer, "%f,%f", &desiredSpeed0, &desiredSpeed1);
+	float convertedSpeed0 = desiredSpeed0*CONVERSION;
+	float convertedSpeed1 = desiredSpeed1*CONVERSION;
+	motor0.pid(convertedSpeed0);
+	motor1.pid(convertedSpeed1);
+	sprintf((char*)buf, ">Speed:%08f,%08f", motor0.getCurrentRevPerTick()/CONVERSION, motor1.getCurrentRevPerTick()/CONVERSION);
+	HAL_UART_Transmit_DMA(&huart1, buf, 26);
+}
+
+void Robot::init(){
+	debug.info("Init ESP8266 begin");
+	HAL_Delay(10000);
+	debug.info("Init ESP8266 end");
+	motor0.setEncoder(&encoder0);
+	motor1.setEncoder(&encoder1);
+	debug.info("Init timers begin");
+	//HAL_TIM_Encoder_Start(&htim1, TIM_CHANNEL_1);
+	HAL_TIM_Encoder_Start(&htim1, TIM_CHANNEL_ALL);
+	//HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_1);
+	HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL);
+	HAL_TIM_Base_Start(&htim2);
+	HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_1);
+	HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_2);
+	HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3);
+	HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4);
+	HAL_TIM_Base_Start_IT(&htim4);
+	HAL_UARTEx_ReceiveToIdle_DMA(&huart1, rxBuffer, 256);
+	debug.info("Init timers end");
+}

Code/STM32/Core/Src/Components/Robot.hpp

@@ -0,0 +1,34 @@
+/*
+ * Robot.hpp
+ *
+ *  Created on: Mar 17, 2023
+ *      Author: Gabriel
+ */
+
+#ifndef SRC_COMPONENTS_ROBOT_HPP_
+#define SRC_COMPONENTS_ROBOT_HPP_
+
+#include "main.h"
+#include "BTS7960B.hpp"
+#include "Encoder.hpp"
+
+class Robot {
+public:
+	Robot();
+	virtual ~Robot();
+	void uartCallback();
+	void controlCallback();
+	void init();
+private:
+	const float reductionRatio = 30;
+	const float wheelRadius = 73.025;	//mm
+	const float ticksPerSecond = 100;
+	uint8_t rxBuffer[1500];
+	uint8_t txBuffer[1500];
+	Encoder encoder0 = Encoder(&(TIM1->CNT), &(TIM1->ARR), 52);
+	Encoder encoder1 = Encoder(&(TIM3->CNT), &(TIM3->ARR), 52);
+	BTS7960B motor0 = BTS7960B(&(TIM2->CCR1), &(TIM2->CCR2), &(TIM2->ARR), &(TIM2->ARR), GPIOB, GPIO_PIN_4, GPIOB, GPIO_PIN_5);
+	BTS7960B motor1 = BTS7960B(&(TIM2->CCR3), &(TIM2->CCR4), &(TIM2->ARR), &(TIM2->ARR), GPIOB, GPIO_PIN_0, GPIOB, GPIO_PIN_1);
+};
+
+#endif /* SRC_COMPONENTS_ROBOT_HPP_ */

Code/STM32/Core/Src/Components/Start.cpp

@@ -1,8 +1,6 @@
 #include "Start.hpp"
 #include "SerialDebug.hpp"
-#include "ESP8266.hpp"
-#include "BTS7960B.hpp"
-#include <cstring>
+#include "Robot.hpp"
 
 extern UART_HandleTypeDef huart1;
 extern UART_HandleTypeDef huart2;
@@ -12,25 +10,15 @@ extern TIM_HandleTypeDef htim3;
 extern TIM_HandleTypeDef htim4;
 
 SerialDebug debug(&huart2, 32);
-//ESP8266 esp0(&huart1);
-BTS7960B motor0(&(TIM2->CCR1), &(TIM2->CCR2), GPIOB, GPIO_PIN_4, GPIOB, GPIO_PIN_5);
-BTS7960B motor1(&(TIM2->CCR3), &(TIM2->CCR4), GPIOB, GPIO_PIN_0, GPIOB, GPIO_PIN_1);
+Robot robot;
 
 //Temporary variables begin
-uint8_t buf[1500];
-float error = 0;
-float lastError = 0;
-float derror = 0;
-float ierror = 0;
-float deltaT = 33;
-float kp = 16.0715;
-float ki = 0.028822;
-float kd = 1750.3479;
+
 //Temporary Variables end
 
 void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim){
 	if(htim==&htim4){
-
+		robot.controlCallback();
 	}
 }
 
@@ -45,48 +33,15 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
 }
 
 void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size){
-	HAL_GPIO_TogglePin(LED_BUILTIN_GPIO_Port, LED_BUILTIN_Pin);
-	HAL_UARTEx_ReceiveToIdle_DMA(&huart1, buf, 256);
-}
-
-void pid(int32_t input){
-	lastError = error;
-	error = input - (int32_t)TIM3->CNT;
-	derror = error - lastError;
-	ierror += error;
-	if(ierror > 2047){
-		ierror = 2047;
-	}else if(ierror < -2048){
-		ierror = -2048;
+	if(huart == &huart1){
+		robot.uartCallback();
 	}
-	float pid = kp*error + ki*ierror*deltaT + kd*derror/deltaT;
-	if(pid > 2047){
-		pid = 2047;
-	}else if(pid<-2047){
-		pid = -2047;
-	}
-	motor1.setSpeed((int32_t)pid);
 }
 
 void start(){
 	debug.setLevel(SerialDebug::DEBUG_LEVEL_INFO);
 	debug.info("-----Init-----");
-	debug.info("Init timers begin");
-	//HAL_TIM_Encoder_Start(&htim1, TIM_CHANNEL_1);
-	HAL_TIM_Encoder_Start(&htim1, TIM_CHANNEL_ALL);
-	//HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_1);
-	HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL);
-	HAL_TIM_Base_Start(&htim2);
-	HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_1);
-	HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_2);
-	HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3);
-	HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4);
-	HAL_TIM_Base_Start(&htim4);
-	debug.info("Init timers end");
-	debug.info("Init ESP8266 begin");
-	HAL_Delay(1000);
-	HAL_UARTEx_ReceiveToIdle_DMA(&huart1, buf, 256);
-	debug.info("Init ESP8266 end");
+	robot.init();
 	while(true){
 
 	}

Code/STM32/Core/Src/main.c

@@ -198,7 +198,7 @@ static void MX_TIM1_Init(void)
   htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
   htim1.Init.RepetitionCounter = 0;
   htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
-  sConfig.EncoderMode = TIM_ENCODERMODE_TI1;
+  sConfig.EncoderMode = TIM_ENCODERMODE_TI12;
   sConfig.IC1Polarity = TIM_ICPOLARITY_RISING;
   sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI;
   sConfig.IC1Prescaler = TIM_ICPSC_DIV1;
@@ -318,7 +318,7 @@ static void MX_TIM3_Init(void)
   htim3.Init.Period = 65535;
   htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
   htim3.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
-  sConfig.EncoderMode = TIM_ENCODERMODE_TI1;
+  sConfig.EncoderMode = TIM_ENCODERMODE_TI12;
   sConfig.IC1Polarity = TIM_ICPOLARITY_RISING;
   sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI;
   sConfig.IC1Prescaler = TIM_ICPSC_DIV1;

Code/STM32/F103C8-PFC.ioc

@@ -233,6 +233,8 @@ SH.S_TIM3_CH1.0=TIM3_CH1,Encoder_Interface
 SH.S_TIM3_CH1.ConfNb=1
 SH.S_TIM3_CH2.0=TIM3_CH2,Encoder_Interface
 SH.S_TIM3_CH2.ConfNb=1
+TIM1.EncoderMode=TIM_ENCODERMODE_TI12
+TIM1.IPParameters=EncoderMode
 TIM2.AutoReloadPreload=TIM_AUTORELOAD_PRELOAD_ENABLE
 TIM2.Channel-PWM\ Generation1\ CH1=TIM_CHANNEL_1
 TIM2.Channel-PWM\ Generation2\ CH2=TIM_CHANNEL_2
@@ -241,6 +243,8 @@ TIM2.Channel-PWM\ Generation4\ CH4=TIM_CHANNEL_4
 TIM2.IPParameters=Channel-PWM Generation1 CH1,Channel-PWM Generation2 CH2,Channel-PWM Generation3 CH3,Channel-PWM Generation4 CH4,Period,Prescaler,AutoReloadPreload
 TIM2.Period=2047
 TIM2.Prescaler=63
+TIM3.EncoderMode=TIM_ENCODERMODE_TI12
+TIM3.IPParameters=EncoderMode
 TIM4.IPParameters=Prescaler,Period
 TIM4.Period=9999
 TIM4.Prescaler=71