diff --git a/Core/Inc/dribbler_board/peripherals/dribbler.h b/Core/Inc/dribbler_board/peripherals/dribbler.h index a5a024e1..8458d38a 100644 --- a/Core/Inc/dribbler_board/peripherals/dribbler.h +++ b/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_ */ diff --git a/Core/Src/dribbler_board/dribbler_board.c b/Core/Src/dribbler_board/dribbler_board.c index 9ebd63c6..28ead7f2 100644 --- a/Core/Src/dribbler_board/dribbler_board.c +++ b/Core/Src/dribbler_board/dribbler_board.c @@ -1,9 +1,16 @@ #include "dribbler_board.h" +#include +#include 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); } diff --git a/Core/Src/dribbler_board/peripherals/ballSensor.c b/Core/Src/dribbler_board/peripherals/ballSensor.c index dc2b8ab6..07c30042 100644 --- a/Core/Src/dribbler_board/peripherals/ballSensor.c +++ b/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; diff --git a/Core/Src/dribbler_board/peripherals/dribbler.c b/Core/Src/dribbler_board/peripherals/dribbler.c index cbe1c14a..f3db1a3b 100644 --- a/Core/Src/dribbler_board/peripherals/dribbler.c +++ b/Core/Src/dribbler_board/peripherals/dribbler.c @@ -1,8 +1,23 @@ #include "dribbler.h" +#include + 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; + } +}