testing electromagnets, compiles but doesn't work

src/deck/drivers/src/Kbuild

@@ -20,4 +20,5 @@ obj-$(CONFIG_DECK_ZRANGER)       += zranger.o
 obj-$(CONFIG_DECK_ZRANGER2)      += zranger2.o
 # obj-y                            += loadcell_nau7802.o
 # obj-y                            += rpm.o
-# obj-y                            += acs37800.o
+# obj-y                            += acs37800.o
+obj-y                            += deck_servo.o

src/deck/drivers/src/deck_electromagnet.c

@@ -0,0 +1,78 @@
+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2023 BitCraze AB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, in version 3.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * deck_electromagnet.c - Deck driver for the electromagnet deck
+ */
+
+#define DEBUG_MODULE "DeckElectromagnet"
+#include "debug.h"
+
+#include "deck.h"
+#include "param.h"
+
+#define GATE_PIN    DECK_GPIO_IO1
+uint8_t state = 0;
+
+static void activateCallback(void);
+
+static void initializeDeckElectromagnet()
+{
+	DEBUG_PRINT("Initializing electromagnet deck!\n");
+
+	// Configure pin IO1 as an output with pull-down resistor
+	pinMode(GATE_PIN, OUTPUT);
+
+	// initialize with given default state
+	activateCallback();
+
+	// Test: Activate electromagnet
+	// digitalWrite(GATE_PIN, HIGH);
+
+	DEBUG_PRINT("Electromagnet deck initialized successfully!\n");
+}
+
+static void activateCallback(void)
+{
+	if (state) {
+		DEBUG_PRINT("Activating electromagnet deck!\n");
+		digitalWrite(GATE_PIN, HIGH);
+	} else {
+		DEBUG_PRINT("Deactivating electromagnet deck!\n");
+		digitalWrite(GATE_PIN, LOW);
+	}
+}
+
+static const DeckDriver driverElectromagnet = {
+	.vid = 0,
+  	.pid = 0,
+	.name = "deckElectromagnet",
+	.usedGpio = DECK_USING_IO_1,
+	.init = initializeDeckElectromagnet,
+};
+
+DECK_DRIVER(driverElectromagnet);
+
+// add a parameter to the deck driver
+PARAM_GROUP_START(electromagnet)
+PARAM_ADD_WITH_CALLBACK(PARAM_UINT8, activate, &state, &activateCallback)
+PARAM_GROUP_STOP(electromagnet)

src/deck/drivers/src/deck_servo.c

@@ -0,0 +1,158 @@
+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2023 BitCraze AB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, in version 3.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * deck_servo.c - Deck driver for the servo deck
+ */
+
+#define DEBUG_MODULE "DeckServo"
+#include "debug.h"
+
+#include "deck.h"
+#include "param.h"
+
+#include "servo.h"
+
+// linear servo ratios 
+// right servo definitions: shaft looking up -> arm at top == 100% length, arm at bottom == 0% length)
+// the following comments for the ratios apply for the right servo only
+#define ratio_500us 	6 	// 0.5 ms pulse at 50 Hz 	-> 100% length (GETS HOT, CRITICAL) -> (CABLES) BOTTOM |-----...----x| TOP (SHAFT)
+#define ratio_700us 	9 	// 0.7 ms pulse at 50 Hz 	-> 95% length (OK, COOL)			-> (CABLES) BOTTOM |-----...---x-| TOP (SHAFT)
+#define ratio_750us 	10 	// 0.75 ms pulse at 50 Hz 	-> 90% length						-> (CABLES) BOTTOM |-----...--x--| TOP (SHAFT)
+#define ratio_850us 	11 	// 0.85 ms pulse at 50 Hz 	-> ...								-> (CABLES) BOTTOM |-----...-----| TOP (SHAFT)
+#define ratio_950us 	12 	// 0.95 ms pulse at 50 Hz 	-> ...								-> (CABLES) BOTTOM |-----...-----| TOP (SHAFT)	
+#define ratio_1000us 	13 	// 1.0 ms pulse at 50 Hz 	-> ...								-> (CABLES) BOTTOM |-----...-----| TOP (SHAFT)
+#define ratio_1500us 	19 	// 1.5 ms pulse at 50 Hz 	-> ...								-> (CABLES) BOTTOM |-----...-----| TOP (SHAFT)
+#define ratio_2000us 	26 	// 2.0 ms pulse at 50 Hz 	-> 20% length						-> (CABLES) BOTTOM |----x...-----| TOP (SHAFT)
+#define ratio_2300us 	29 	// 2.3 ms pulse at 50 Hz 	-> 5% length						-> (CABLES) BOTTOM |-x---...-----| TOP (SHAFT)
+#define ratio_2500us 	32 	// 2.5 ms pulse at 50 Hz 	-> 0% length (OK, COOL)				-> (CABLES) BOTTOM |x----...-----| TOP (SHAFT)
+
+// ratio limits of the linear actuators (top == shaft end)
+#define ratioRightTop 		ratio_700us
+#define ratioRightBottom 	ratio_2300us
+#define ratioLeftTop 		ratio_2300us	// mirrored
+#define ratioLeftBottom 	ratio_850us		// mirrored, but with ratio_700us, the left servo gets crititcally hot
+
+// normal servo info (currently not in use)
+//  500 us -> 190 deg
+// 1000 us -> 135 deg
+// 1500 us -> 100 deg
+// 2000 us ->  60 deg
+// 2500 us ->  10 deg
+
+uint8_t ratio = 0;
+
+uint16_t frequency = 50;
+uint16_t frequencyDefault = 50;
+
+uint8_t state = 0;
+
+static void testServo(void);
+static void activateServo(void);
+static void deactivateServo(void);
+static void setRatioCallback(void);
+static void setFrequencyCallback(void);
+
+static void initializeDeckServo()
+{
+	DEBUG_PRINT("Initializing servo deck!\n");
+
+	// initialize the PWM pin TX1
+    servoInit();
+
+    DEBUG_PRINT("Servo deck initialized successfully!\n");
+
+	// test the servo deck
+	testServo();
+}
+
+static void testServo(void)
+{
+	DEBUG_PRINT("Starting servo deck test!\n");
+
+	servoSetFreq(frequencyDefault);
+	activateServo();  					// CLOSE GRIPPER with right servo
+	deactivateServo(); 					// OPEN GRIPPER with right servo
+
+	DEBUG_PRINT("Ending servo deck test!\n");
+}
+
+static void activateCallback(void)
+{
+	if (state) {
+		activateServo(); 				// CLOSE GRIPPER with right servo
+	} else {
+		deactivateServo(); 				// OPEN GRIPPER with right servo
+	}
+}
+
+static void activateServo()
+{	
+	DEBUG_PRINT("Activating servo!\n");
+
+	servoSetFreq(frequencyDefault);
+	servoSetRatio(ratioRightBottom); 	// CLOSE GRIPPER with right servo
+}
+
+static void deactivateServo()
+{
+	DEBUG_PRINT("Deactivating servo!\n");
+
+	servoSetFreq(frequencyDefault);
+	servoSetRatio(ratioRightTop); 		// OPEN GRIPPER with right servo
+}
+
+static void setRatioCallback(void)
+{	
+	DEBUG_PRINT("Setting servo ratio to %d!\n", ratio);
+
+	servoSetRatio(ratio);
+}
+
+static void setFrequencyCallback(void)
+{
+	DEBUG_PRINT("Setting servo frequency to %d!\n", frequency);
+
+	servoSetFreq(frequency);
+}
+
+static const DeckDriver driverServo = {
+	.vid = 0,
+  	.pid = 0,
+	.name = "deckServo",
+
+	.usedPeriph = DECK_USING_PA2 | DECK_USING_TIMER5,
+	.usedGpio = 0,
+
+	.init = initializeDeckServo,
+};
+
+DECK_DRIVER(driverServo);
+
+// servo deck parameters
+PARAM_GROUP_START(servo)
+
+PARAM_ADD_WITH_CALLBACK(PARAM_UINT8, activate, &state, &activateCallback)
+PARAM_ADD_WITH_CALLBACK(PARAM_UINT8, setRatio, &ratio, &setRatioCallback)
+PARAM_ADD_WITH_CALLBACK(PARAM_UINT16, setFrequency, &frequency, &setFrequencyCallback)
+
+PARAM_GROUP_STOP(servo)

src/drivers/interface/servo.h

@@ -0,0 +1,59 @@
+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2011-2012 Bitcraze AB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, in version 3.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * Servo.h - Servo motor driver header file (based on motors.h & piezo.h)
+ *
+ * The motors PWM ratio is a value on 16 bits (from 0 to 0xFFFF)
+ * the functions of the driver will make the conversions to the actual PWM
+ * precision (ie. if the precision is 8bits 0xFFFF and 0xFF00 are equivalents).
+ *
+ * The precision is set in number of bits by the define MOTORS_PWM_BITS
+ * The timer prescaler is set by MOTORS_PWM_PRESCALE
+ */
+#ifndef __SERVO_H__
+#define __SERVO_H__
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "config.h"
+
+/******** Defines ********/
+
+/*** Public interface ***/
+
+/**
+ * Servo Initialization. Configures TX1 as a PWM pin.
+ */
+void servoInit();
+
+/**
+ * Set servo PWM ratio/power.
+ */
+void servoSetRatio(uint8_t ratio);
+
+/**
+ * Set servo PWM frequency in hertz.
+ */
+void servoSetFreq(uint16_t freq);
+
+#endif /* __SERVO_H__ */

src/drivers/src/Kbuild

@@ -25,3 +25,4 @@ obj-y += vl53l0x.o
 obj-y += vl53l1x.o
 obj-y += watchdog.o
 obj-y += ws2812_cf2.o
+obj-y += servo.o