Merge branch 'memduh' into 'WS2021-clean-up'

Cleaned up controller code, edited repetitive PID-Code

See merge request !53

src/controller/Makefile

@@ -9,7 +9,7 @@ obj-$(CONFIG_CONTROLLER) += controller_rcLEDTask.o
 obj-$(CONFIG_CONTROLLER) += controller_blinkerTask.o
 obj-$(CONFIG_CONTROLLER) += controller_btn.o
 obj-$(CONFIG_CONTROLLER) += controller_emergency.o
-obj-$(CONFIG_PID_CONTROLLER) += controller_throttle.o
+obj-$(CONFIG_CONTROLLER) += controller_throttle.o
 obj-$(CONFIG_CONTROLLER) += controller_stateMaschine.o
 obj-$(CONFIG_CONTROLLER) += controller_updateDriveParams.o
 #obj-$(CONFIG_CONTROLLER) += 

src/controller/controller_drive.c

@@ -76,11 +76,11 @@ void controller_handleDrive(struct controller *ctrl) {
 			if (throttle > 0.0000001) {
 				motor_set(ctrl->motor,
 
-				          CONTROLLER_THROTTLE_ZERO + CONTROLLER_THROTTLE_SLOW);
+				          CONTROLLER_THROTTLE_ZERO + CONTROLLER_THROTTLE_SLOW + ctrl->desired_throttle);
 			} else if (throttle < -0.0000001) {
 				motor_set(ctrl->motor,
 				          CONTROLLER_THROTTLE_ZERO -
-				          CONTROLLER_THROTTLE_SLOW);
+				          CONTROLLER_THROTTLE_SLOW - ctrl->desired_throttle);
 			} else {
 				motor_set(ctrl->motor,
 				          CONTROLLER_THROTTLE_ZERO);

src/controller/controller_prv.h

@@ -105,10 +105,9 @@ struct controller {
 	OS_DEFINE_TASK(blinkerTask, 250);
 	struct gpio_pin *blinkerL;
 	struct gpio_pin *blinkerR;
-
 	struct gpio_pin *brake;
-	bool isBraking;
 
+	bool isBraking;
 	bool first;
 
 	uint32_t lasttime;
@@ -121,9 +120,7 @@ struct controller {
 	/* values from handle_* */
 	float desired_throttle;
 	uint32_t desired_steering;
-
 	uint32_t latest_throttle;
-
 	enum controller_direction dir;
 
 #ifdef CONFIG_PID_CONTROLLER
@@ -187,6 +184,7 @@ void controller_btnPressTask(void *data);
  * \param waittime Max Waittime
  * \return -1 on timeout 0 on ok
  */
+
 int32_t controller_setAndClearStatusBits(struct controller *ctrl, uint32_t clear, uint32_t set, TickType_t waittime);
 /**
  * Clear Status Bits
@@ -195,6 +193,7 @@ int32_t controller_setAndClearStatusBits(struct controller *ctrl, uint32_t clear
  * \param wait_time Max Waittime
  * \return -1 on timeout 0 on ok
  */
+
 int32_t controller_clearStatusBits(struct controller *ctrl, uint32_t status, TickType_t wait_time);
 /**
  * Toggle Status Bit
@@ -203,6 +202,7 @@ int32_t controller_clearStatusBits(struct controller *ctrl, uint32_t status, Tic
  * \param wait_time Max Waittime
  * \return -1 on timeout 0 on ok
  */
+
 int32_t controller_toggleStatusBits(struct controller *ctrl, uint32_t status, TickType_t wait_time);
 void controller_handleDrive(struct controller *ctrl);
 void controller_handleRC(struct controller *ctrl);

src/controller/controller_task.c

@@ -25,7 +25,6 @@ void controller_task(void *data) {
 			controller_handleShutdown(ctrl);
 		}
 
-#ifdef CONFIG_PID_CONTROLLER
 		controller_updateDriveParams(ctrl, ctrl->desired_throttle, ctrl->desired_steering);
 
 		/*
@@ -34,13 +33,14 @@ void controller_task(void *data) {
 		 */
 		if (status & SHM_TELEMETRIE_STATUS_DRIVE || status & SHM_TELEMETRIE_STATUS_RC) {
 			if (ctrl->dir == CONTROLLER_NOT_MOVEING) {
+#ifdef CONFIG_PID_CONTROLLER
 				pid_reset(ctrl->pid);
+#endif
 			}
 			controller_motor_throttle_set(ctrl, ctrl->desired_throttle);
 			motor_set(ctrl->servo, ctrl->desired_steering);
 		}
 		controller_updateStateMaschine(ctrl, ctrl->latest_throttle);
-#endif /* ifndef CONFIG_PID_CONTROLLER */
 
 		/* wait */
 		vTaskDelayUntil(&lastWakeTime, 10 / portTICK_PERIOD_MS);

src/controller/controller_throttle.c

@@ -7,6 +7,7 @@
  * @arg setpoint the aspired speed in m/s
  */
 int32_t controller_motor_throttle_set(struct controller *ctrl, float setpoint) {
+#ifdef CONFIG_PID_CONTROLLER
 	uint32_t time;
 	int32_t ret;
 	float pid_set;
@@ -143,6 +144,6 @@ int32_t controller_motor_throttle_set(struct controller *ctrl, float setpoint) {
 	CONFIG_ASSERT(ret >= 0);
 	/*set motor with pid_set*/
 	motor_set(ctrl->motor, (int32_t) pid_set);
-
+#endif /* ifdef CONFIG_PID_CONTROLLER */
 	return 0;
 }