Merge pull request #45 from RoboTeamTwente/dribbler_speed

Dribbler speed

Core/Inc/dribbler_board/peripherals/dribbler.h

@@ -31,7 +31,8 @@ extern uint32_t encoder_value;
 #define DRIBBLER_TIME_DIFF 0.1F              // Given that the dribbler is updated/controlled at 10Hz, delta time is 0.1s
 #define DRIBBLER_ENCODER_TO_OMEGA ((2. * M_PI * DRIBBLER_GEAR_RATIO) / (DRIBBLER_TIME_DIFF * DRIBBLER_PULSES_PER_ROTATION)) // Conversion factor from number of encoder pulses to dribbler speed [rad/s]
 
-
+// Test the dribbler motor
+void dribbler_test();
 // Initializes the PIDs / encoders / PWM timers
 void dribbler_Init();
 // Initializes the motor driver for the PWM
@@ -46,8 +47,25 @@ bool dribbler_hasBall();
 void dribbler_DeInit();
 // Sets the dribbler speed and makes sure it's within [0,1], if brake is true, the motor will brake if pwm is 0, else it will coast
 void dribbler_SetSpeed(float speed, bool brake);
+// Sets the minimum speed of the dribbler, if brake is true, the motor will brake if pwm is 0, else it will coast
+void dribbler_SetMinSpeed(bool brake);
+// Sets the idle speed of the dribbler, if brake is true, the motor will brake if pwm is 0, else it will coast
+void dribbler_SetIdleSpeed(bool brake);
+// Sets the maximum speed of the dribbler, if brake is true, the motor will brake if pwm is 0, else it will coast
+void dribbler_SetMaxSpeed(bool brake);
+
+float dribbler_GetMinSpeed(bool brake);
+float dribbler_GetIdleSpeed(bool brake);
+float dribbler_GetMaxSpeed(bool brake);
+
+// Returns if the dribbler has an encoder
+bool dribbler_hasEncoder();
 // Returns the latest encoder measurement
 uint32_t dribbler_GetEncoderMeasurement();
+// Updates the dribbler speed based on the encoder
+void dribbler_UpdateEncoderSpeed();
+// Returns the dribbler speed in 
+float dribbler_GetEncoderSpeed();
 
 
 #endif /* DRIBBLER_DRIBBLER_H_ */

Core/Src/dribbler_board/dribbler_board.c

@@ -1,9 +1,16 @@
 #include "dribbler_board.h"
+#include <stdlib.h>
+#include <string.h>
 
 volatile bool BOARD_INITIALIZED = false;
 
 void MCP_Process_Message(mailbox_buffer *to_Process);
 void MCP_Send_Im_Alive();
+void do_send_ballState();
+
+void no_encoder_control();
+void has_encoder_control();
+
 
 // Outgoing MCP headers
 CAN_TxHeaderTypeDef dribblerAliveHeaderToTop = {0};
@@ -35,6 +42,25 @@ uint8_t ball_counter = 0;
 uint32_t current_beat = 0;
 uint32_t heart_beat_10ms = 0;
 
+uint8_t Uart_Tx_Buffer[40] = {0};
+
+//Dribbler control
+#define CONTROL_TIMER_PERIOD 0.001f
+float setpoint = 0;
+float PWM = 0.0f;
+float speed = 0; 
+float timestamp = 0;
+
+float Kp = 0.002f;  
+float Ki = 0.00f; 
+float Kd = 0.00f;  
+
+float previous_error = 0.0f;
+float integral = 0.0f;
+
+int state = 0;
+
+// #define LOGGING //uncomment if you want output
 
 /* ======================================================== */
 /* ==================== INITIALIZATION ==================== */
@@ -43,6 +69,7 @@ void init(){
     HAL_IWDG_Refresh(&hiwdg);
     // Peripherals
     HAL_TIM_Base_Start_IT(CONTROL_TIMER); //start the timer used for the control loop
+    HAL_TIM_Encoder_Start_IT(&htim2, TIM_CHANNEL_ALL); //enable timer for the encoder
     LOG_init();
     dribbler_Init();
     ballsensor_init();
@@ -78,6 +105,9 @@ uint8_t robot_get_Channel(){
 /* =================================================== */
 /* ==================== MAIN LOOP ==================== */
 /* =================================================== */
+
+
+
 void loop(){
     HAL_IWDG_Refresh(&hiwdg);
     MCP_timeout();
@@ -91,6 +121,11 @@ void loop(){
         MCP_to_process = false;
 	}
     do_send_ballState();
+
+#ifdef LOGGING
+    sprintf((char*) Uart_Tx_Buffer, "S:%.2f,D:%.2f,P:%.2f,T:%.4f\n",setpoint, speed, PWM, timestamp);
+    HAL_UART_Transmit_DMA(&huart1, Uart_Tx_Buffer, sizeof (Uart_Tx_Buffer)-1);
+#endif
 }
 
 /* ============================================= */
@@ -161,29 +196,80 @@ void do_send_ballState(){
         MCP_Send_Ball_State();
     }
 }
-
 void control_dribbler_callback() { 
 
+    dribbler_UpdateEncoderSpeed();
+    timestamp += CONTROL_TIMER_PERIOD;
+
     set_Pin(LED1, ballsensor_hasBall());
     set_Pin(LED2, dribbler_hasBall());
+
     do_send_ballState();
-    if (dribblerCommand.dribblerOn) {
-        if(ballsensor_hasBall()){
-            ball_counter = 0;
-            dribbler_SetSpeed(0.8f, 1);
-            return;
-        }
-        else if (ball_counter < 5){
-            ball_counter = ball_counter + 1;
-            dribbler_SetSpeed(0.2f, 1);
-            return;
-        } else if (dribblerCommand.SystemTest) {
-            dribbler_SetSpeed(0.5f, 1);
-            return;
+
+    if (dribblerCommand.dribblerOn){
+        if(dribbler_hasEncoder()){
+            has_encoder_control();
+        } else{
+            no_encoder_control();
         }
     }
-    dribbler_SetSpeed(0.0f, 1);
+
+} 
+
+
+
+
+void has_encoder_control() {
+
+
+    if(ballsensor_hasBall()){
+            setpoint = 500;
+            state = 1;
+    } else {
+            setpoint = 0;
+            state = 2;
+    }
+
+
+    speed = dribbler_GetEncoderSpeed();
+
+    float error = setpoint - (float)fabs(speed);
+
+    integral += error * CONTROL_TIMER_PERIOD;
+
+    float derivative = (error - previous_error) / CONTROL_TIMER_PERIOD;
 
+    float output = (Kp * error) + (Ki * integral) + (Kd * derivative);
+
+    PWM += output;
+
+    if (PWM > 1.0f) {
+        PWM = 1.0f;
+    } else if (PWM < 0.0f) {
+        PWM = 0.0f;
+    }
+
+    previous_error = error;
+
+    dribbler_SetSpeed(PWM,1);
+}
+
+
+void no_encoder_control(){
+    if(ballsensor_hasBall()){
+        ball_counter = 0;
+        dribbler_SetMaxSpeed(1);
+        return;
+    }
+    else if (ball_counter < 5){
+        ball_counter = ball_counter + 1;
+        dribbler_SetIdleSpeed(1);
+        return;
+    } else if (dribblerCommand.SystemTest) {
+        dribbler_SetSpeed(0.5f, 1);
+        return;
+    } 
+        dribbler_SetSpeed(0.0f, 1);
 }
 
 

Core/Src/dribbler_board/peripherals/ballSensor.c

@@ -2,7 +2,7 @@
 
 uint16_t ADC2_buffer[DMA_bufferSize];
 uint8_t counter = 0;
-bool ballSensor_isWorking = false;
+bool ballSensor_isWorking = true;
 bool IR_State = false;
 
 

Core/Src/dribbler_board/peripherals/dribbler.c

@@ -1,8 +1,23 @@
 #include "dribbler.h"
+#include <stdlib.h>
+
 
 
 uint16_t dribbler_current_Buffer[current_Buffer_Size];
 float current_limit;
+bool motor_reversed = false;
+bool has_encoder = true;
+float dribbler_min_speed_PWM = 0.1f;
+float dribbler_idle_speed_PWM = 0.2f;
+float dribbler_max_speed_PWM = 0.8f;
+
+float dribbler_min_speed = 0;
+float dribbler_idle_speed = 0;
+float dribbler_max_speed = 0;
+
+int calculateDifference(int value1, int value2, int maxvalue);//calculated difference between 2 values, with wrapping
+float dribbler_speed = 0;
+
 void dribbler_Init(){
 	current_limit=1.0f; // [A] 0.32A is the maximum continuous current of the dribbler motor
 	HAL_TIM_Base_Start(PWM_DRIBBLER);
@@ -19,6 +34,7 @@ void dribbler_motor_Init(){
 
 	// For the voltage
 	dribbler_setCurrentLimit(current_limit);
+	dribbler_test(); //Test the motor and see if it's reversed or not
 }
 
 /**
@@ -34,6 +50,43 @@ void dribbler_motor_Init(){
  * R_IPROPI = 1000 (Ω) V_VREF = 3.3 (V), A_IPROPI = 1500 (μA/A)
  *
 */
+void dribbler_test(){
+	dribbler_SetSpeed(0.15f, 1);
+	HAL_Delay(100);
+
+	if(dribbler_GetEncoderSpeed() == 0){
+		has_encoder = false;
+	} else if(dribbler_GetEncoderSpeed() < 0){
+		motor_reversed = false;
+	} else{
+		motor_reversed = true;
+	}
+	dribbler_SetSpeed(0.0f, 1);
+	HAL_Delay(100);
+
+	if(has_encoder){
+		for(float f = 0.0f; f <= 0.2f; f += 0.01f){
+			dribbler_SetSpeed(f, 1);
+			HAL_Delay(15);
+			dribbler_min_speed = dribbler_GetEncoderSpeed();
+			if(fabs(dribbler_min_speed) > 20){
+				dribbler_min_speed_PWM = f;
+				break;
+			}
+		}
+		HAL_Delay(10);
+	}
+
+
+	// dribbler_SetIdleSpeed(1);
+	// HAL_Delay(100);
+	// dribbler_idle_speed = dribbler_GetEncoderSpeed();
+	// dribbler_SetMaxSpeed(1);
+	// HAL_Delay(100);
+	// dribbler_max_speed = dribbler_GetEncoderSpeed();
+	dribbler_SetSpeed(0.0f, 1);
+
+}
 void dribbler_setCurrentLimit(float value){
 	// The value you put in is the DOR Vout=DOR*(Vref/4095) where Vref is the reference voltage of the DAC (3.3V)
 	//float V_set= (float)(value * 1.5f); //
@@ -50,7 +103,15 @@ void dribbler_setCurrentLimit(float value){
 float dribbler_getCurrent(){
 	// For now we get the most recent reading
 	// Value=ADC_Value/4095*Vref/1.5 where 1.5 is the conversion factor in combination with the resistor /4095*3.3/1.5
-	uint32_t current_temp = dribbler_current_Buffer[0];
+
+	//average the first 10 elements of the dribbler_current_Buffer
+	uint32_t current_sum = 0;
+	for (int i = 0; i < current_Buffer_Size; i++){
+		current_sum += dribbler_current_Buffer[i];
+	}
+
+	uint32_t current_temp =  current_sum / current_Buffer_Size;
+
 	float currentA = ((float)(current_temp-232))/1939;
 	return currentA;
 }
@@ -68,6 +129,12 @@ void dribbler_DeInit(){
 }
 
 void dribbler_SetSpeed(float speed, bool brake){
+
+	if(motor_reversed){
+		speed = -speed; // The motor is mounted in reverse
+	}
+
+
 	if (brake){ // The motor is in braking mode
 		if (speed > 0){
 			set_PWM_dribbler(&PWM_Dribbler_b, 1);
@@ -90,8 +157,65 @@ void dribbler_SetSpeed(float speed, bool brake){
 	}
 }
 
+void dribbler_SetMinSpeed(bool brake){
+	dribbler_SetSpeed(dribbler_min_speed_PWM, brake);
+}
+void dribbler_SetIdleSpeed(bool brake){
+	dribbler_SetSpeed(dribbler_idle_speed_PWM, brake);
+}
+void dribbler_SetMaxSpeed(bool brake){
+	dribbler_SetSpeed(dribbler_max_speed_PWM, brake);
+}
+
+float dribbler_GetMinSpeed(bool brake){
+	return dribbler_min_speed;
+}
+float dribbler_GetIdleSpeed(bool brake){
+	return dribbler_idle_speed;
+}
+float dribbler_GetMaxSpeed(bool brake){
+	return dribbler_max_speed;
+}
+
+
+bool dribbler_hasEncoder(){
+	return has_encoder;
+}
+
 
 uint32_t dribbler_GetEncoderMeasurement(){
-	encoder_value = get_encoder(PWM_Dribbler_a);
+	uint32_t encoder_value = __HAL_TIM_GET_COUNTER(ENC_DRIBBLER);
 	return encoder_value;
 }
+
+void dribbler_UpdateEncoderSpeed(){
+	static uint32_t last_encoder_value = 0;
+	uint32_t current_encoder_value = dribbler_GetEncoderMeasurement();
+	float speed = (float)(calculateDifference(last_encoder_value, current_encoder_value, 65535))*(2.4576f);
+	last_encoder_value = current_encoder_value;
+	dribbler_speed =  speed;
+}
+
+float dribbler_GetEncoderSpeed(){
+	return dribbler_speed;
+}
+
+
+int calculateDifference(int value1, int value2, int maxvalue) {
+    int directDiff = value2 - value1;
+    int wrapDiff;
+
+    // Calculate wrap-around difference
+    if (value1 > value2) {
+        wrapDiff = (maxvalue - value1 + value2 + 1);
+    } else {
+        wrapDiff = -(maxvalue - value2 + value1 + 1);
+    }
+
+    // Determine which difference is smaller in magnitude
+    if (abs(directDiff) < abs(wrapDiff)) {
+        return directDiff;
+    } else {
+        return wrapDiff;
+    }
+}