Added thread safety to USB TX, servo control thread, primitive ADC thread, python test codes

servo_controller/src/command_handler.c

@@ -1,5 +1,9 @@
 #include "command_handler.h"
+#include "command_message.h"
 #include "led.h"
+#include "servo.h"
+#include "adc.h"
+#include "usb.h"
 
 #include <zephyr/logging/log.h>
 
@@ -37,6 +41,53 @@ int command_handler(struct command_message_t *msg) {
 			break;
 		}
 
+		case SERVO_SET: {
+			// Set SERVO
+			int servo = msg->data[0];
+			float angle;
+			memcpy(&angle, &msg->data[1], sizeof(float));
+
+			servo_set_angle(servo, angle);
+
+			break;
+		}
+
+		case SERVO_SET_ALL: {
+			// Set ALL SERVO
+			float angles[NUM_SERVO_CHANNELS];
+			for (int i = 0; i < NUM_SERVO_CHANNELS; i++) {
+				int start = i*4;
+				memcpy(&angles[i], &msg->data[start], sizeof(float));
+			}
+
+			servo_set_all_angles(angles);
+
+			break;
+		}
+
+		case ADC_READ: {
+			int channel = msg->data[0];
+			int value = adc_read_id(channel);
+
+			struct command_message_t *reply = usb_get_next_tx_buf();
+			command_create_message(reply, sizeof(value), ADC_READ, (uint8_t *)&value);
+			usb_send_command(reply);
+
+			break;
+		}
+		case ADC_READ_ALL: {
+			int values[NUM_ADC_CHANNELS];
+			for (int i = 0; i < NUM_ADC_CHANNELS; i++) {
+				values[i] = adc_read_id(i);
+			}
+
+			struct command_message_t *reply = usb_get_next_tx_buf();
+			command_create_message(reply, sizeof(values), ADC_READ_ALL, (uint8_t *)values);
+			usb_send_command(reply);
+			
+			break;
+		}
+
 		default: {
 			LOG_WRN("Unknown command received: %d", msg->command);
 			return -EINVAL;