Adamg/airbrakesbattery

AirbrakesDriver/Inc/airbrakes.hpp

@@ -2,8 +2,8 @@
 #define _AIRBRAKES_HPP_
 
 #include "stm32g4xx.h"
-#define AIRBRAKES_NUM_DEPLOYMENT_LEVELS 10
-#define AIRBRAKES_MAX_CURRENT_AMPS 3.0
+#define AIRBRAKES_NUM_DEPLOYMENT_LEVELS 9
+#define AIRBRAKES_MAX_CURRENT_AMPS 10.0
 
 class AirbrakesDriver {
 public:
@@ -24,6 +24,8 @@ class AirbrakesDriver {
 		return enabled;
 	}
 
+	bool OverrideCurrentLevel(uint16_t lvl);
+
 
 private:
 	TIM_HandleTypeDef* servoPWMTimer;
@@ -57,6 +59,19 @@ class AirbrakesDriver {
 
 	const float hz;
 
+	bool hit = false;
+
+	void SetHardwareCycle() {
+		servoPWMTimer->Instance->CCR1 = round(servoPWMTimer->Instance->ARR * currentDutyCycle);
+	}
+
+
+    constexpr static float MAX_VELOCITY = 0.0008;
+    constexpr static float MAX_ACCEL = 0.00002;
+
+    constexpr static float ABS_MIN = 0.000515;
+    constexpr static float ABS_MAX = 0.002485 / 2.1;
+
 
 };
 

AirbrakesDriver/airbrakes.cpp

@@ -18,6 +18,10 @@ AirbrakesDriver::AirbrakesDriver(TIM_HandleTypeDef* servoPWMTimerHandle,
  * Resets current latch and monitors for runaway current for 250ms.
  */
 void AirbrakesDriver::Enable() {
+	if(enabled) {
+		return;
+	}
+
 	HAL_TIM_PWM_Start(servoPWMTimer, TIM_CHANNEL_1);
 	HAL_ADC_Start(servoADCHandle);
 
@@ -34,23 +38,28 @@ void AirbrakesDriver::Enable() {
 	HAL_Delay(10);
 
 	// Immediately stop if current starts running away
-	for(uint16_t i = 0; i < 250; i++) {
+	for(uint16_t i = 0; i < 100; i++) {
 		if(!CurrentGood()) {
 			Disable();
 			return;
 		}
 		HAL_Delay(1);
 	}
+	vel = 0;
 	enabled = true;
 }
 
 /* @brief Disables the driver. Stops ADC sampling and PWM output and disables current.
  */
 void AirbrakesDriver::Disable() {
+	if(!enabled) {
+		return;
+	}
 	HAL_GPIO_WritePin(servoENPort, servoENPin, GPIO_PIN_RESET);
 	HAL_TIM_PWM_Stop(servoPWMTimer, TIM_CHANNEL_1);
 	HAL_ADC_Stop(servoADCHandle);
 	HAL_GPIO_WritePin(servoLatchResetPort, servoLatchResetPin, GPIO_PIN_RESET);
+	vel = 0;
 	enabled = false;
 }
 
@@ -63,7 +72,9 @@ bool AirbrakesDriver::SetTargetLevel(uint8_t level) {
 		return false;
 	}
 
-	return SetTargetDutyCycle(static_cast<float>(level)/(AIRBRAKES_NUM_DEPLOYMENT_LEVELS-1)*(0.0025*hz-0.0005*hz)+0.0005*hz);
+	hit = false;
+	vel = 0;
+	return SetTargetDutyCycle(static_cast<float>(level)/(AIRBRAKES_NUM_DEPLOYMENT_LEVELS-1)*(0.0025*hz-0.0005*hz)/2.1+0.0005*hz);
 }
 
 /* @brief Set the airbrake target duty cycle directly.
@@ -76,6 +87,8 @@ bool AirbrakesDriver::SetTargetDutyCycle(float dutyFrac) {
 		return false;
 	}
 	targetDutyCycle = dutyFrac;
+	hit = false;
+	vel = 0;
 	return true;
 }
 
@@ -98,17 +111,23 @@ float AirbrakesDriver::ReadVoltsADC() {
  */
 bool AirbrakesDriver::TickControlLoop() {
 
-	if(!CurrentGood() || !IsEnabled()) {
+	if(!IsEnabled()) {
+		return false;
+	}
+
+	if(!CurrentGood()) {
 		Disable();
 		return false;
 	}
 
+	if(hit) {
+		return true;
+	}
+
 
-    const static float MAX_VELOCITY = 0.04;
-    const static float MAX_ACCEL = 0.0007;
 
     float distance = targetDutyCycle - currentDutyCycle;
-    float ideal_vel = sqrtf(2.0 * MAX_ACCEL * fabsf(distance))-MAX_ACCEL;
+    float ideal_vel = sqrtf(2.0 * MAX_ACCEL * fabsf(distance));
 
     if (distance < 0) {
         ideal_vel = -ideal_vel;
@@ -134,7 +153,19 @@ bool AirbrakesDriver::TickControlLoop() {
 
     currentDutyCycle += vel;
 
-	servoPWMTimer->Instance->CCR1 = round(servoPWMTimer->Instance->ARR * currentDutyCycle);
+    if(currentDutyCycle < ABS_MIN*hz) {
+    	currentDutyCycle = ABS_MIN*hz;
+    }
+    if(currentDutyCycle > ABS_MAX*hz) {
+    	currentDutyCycle = ABS_MAX*hz;
+    }
+
+    SetHardwareCycle();
+
+	if(abs(currentDutyCycle-targetDutyCycle) < MAX_VELOCITY) {
+		hit = true;
+		vel = 0;
+	}
 
 	return true;
 
@@ -166,6 +197,14 @@ bool AirbrakesDriver::CurrentGood() {
 	return CheckComparatorGood() && (ADCVoltsToCurrent(ReadVoltsADC()) <= AIRBRAKES_MAX_CURRENT_AMPS);
 }
 
+bool AirbrakesDriver::OverrideCurrentLevel(uint16_t lvl) {
+	if(SetTargetLevel(lvl)) {
+		currentDutyCycle = targetDutyCycle;
+		SetHardwareCycle();
+		return true;
+	}
+	return false;
+}
 
 /* @brief Checks the state of the hardware failsafe latch.
  * @return true if the latch is good, i.e. current limit has not been triggered