Add vdml_optical.c

purduesigbots · Sep 25, 2020 · 630b9bd · 630b9bd
1 parent 27ff3c1
commit 630b9bd
Show file tree

Hide file tree

Showing 3 changed files with 124 additions and 6 deletions.
diff --git a/include/pros/apix.h b/include/pros/apix.h
@@ -373,6 +373,7 @@ typedef enum v5_device_e {
 	E_DEVICE_RADIO = 8,
 	E_DEVICE_VISION = 11,
 	E_DEVICE_ADI = 12,
+	E_DEVICE_OPTICAL = 16,
 	E_DEVICE_GENERIC = 129,
 	E_DEVICE_UNDEFINED = 255
 } v5_device_e_t;

diff --git a/include/pros/optical.h b/include/pros/optical.h
@@ -29,17 +29,17 @@ namespace c {
 #endif
 
 typedef struct optical_rgb_s {
-	double green;
 	double red;
+	double green;
 	double blue;
 	double brightness;
 } optical_rgb_s_t;
 
 typedef struct optical_raw_s {
+	uint16_t clear;
 	uint16_t red;
 	uint16_t green;
 	uint16_t blue;
-	uint16_t clear;
 } optical_raw_s_t;
 
 typedef struct optical_gesture_s {
@@ -138,7 +138,7 @@ int32_t optical_get_proximity(uint8_t port);
  * \param port
  *        The V5 Optical Sensor port number from 1-21
  */
-void optical_set_led_pwm(uint8_t port, int32_t value);
+void optical_set_led_pwm(uint8_t port, uint8_t value);
 
 /**
  * Get the pwm value of the White LED on the sensor
@@ -207,7 +207,7 @@ optical_raw_s_t optical_get_raw(uint8_t port);
  * \return gesture value if the operation was successful or PROS_ERR if
  * the operation failed, setting errno.
  */
-int32_t optical_get_gesture(uint32_t port);
+int32_t optical_get_gesture(uint8_t port);
 
 /**
  * Enable gesture detection on the sensor
@@ -220,7 +220,7 @@ int32_t optical_get_gesture(uint32_t port);
  * \param port
  *        The V5 Optical Sensor port number from 1-21
  */
-void optical_enable_gesture(uint32_t index);
+void optical_enable_gesture(uint8_t port);
 
 /**
  * Disable gesture detection on the sensor
@@ -233,7 +233,7 @@ void optical_enable_gesture(uint32_t index);
  * \param port
  *        The V5 Optical Sensor port number from 1-21
  */
-void optical_disable_gesture(uint32_t index);
+void optical_disable_gesture(uint8_t port);
 
 #ifdef __cplusplus
 }

diff --git a/src/devices/vdml_optical.c b/src/devices/vdml_optical.c
@@ -0,0 +1,117 @@
+/**
+ * \file devices/vdml_imu.c
+ *
+ * Contains functions for interacting with the VEX Inertial sensor.
+ *
+ * Copyright (c) 2017-2019, Purdue University ACM SIGBots.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at http://mozilla.org/MPL/2.0/.
+ */
+
+#include <errno.h>
+
+#include "pros/optical.h"
+#include "v5_api.h"
+#include "vdml/registry.h"
+#include "vdml/vdml.h"
+
+double optical_get_hue(uint8_t port) {
+	claim_port_i(port - 1, E_DEVICE_OPTICAL);
+	double rtn = vexDeviceOpticalHueGet(device->device_info);
+	return_port(port - 1, rtn);
+}
+
+double optical_get_saturation(uint8_t port) {
+	claim_port_i(port - 1, E_DEVICE_OPTICAL);
+	double rtn = vexDeviceOpticalSatGet(device->device_info);
+	return_port(port - 1, rtn);
+}
+
+double optical_get_brightness(uint8_t port) {
+	claim_port_i(port - 1, E_DEVICE_OPTICAL);
+	double rtn = vexDeviceOpticalBrightnessGet(device->device_info);
+	return_port(port - 1, rtn);
+}
+
+int32_t optical_get_proximity(uint8_t port) {
+	claim_port_i(port - 1, E_DEVICE_OPTICAL);
+	double rtn = vexDeviceOpticalProximityGet(device->device_info);
+	return_port(port - 1, rtn);
+}
+
+void optical_set_led_pwm(uint8_t port, uint8_t value) {
+	claim_port_i(port - 1, E_DEVICE_OPTICAL);
+	vexDeviceOpticalLedPwmSet(device->device_info, value);
+	return_port(port - 1, 1);
+}
+
+int32_t optical_get_led_pwm(uint8_t port) {
+	claim_port_i(port - 1, E_DEVICE_OPTICAL);
+	int32_t rtn = vexDeviceOpticalLedPwmGet(device->device_info);
+	return_port(port - 1, rtn);
+}
+
+#define RGB_ERR_INIT \
+	{ .red = PROS_ERR_F, .green = PROS_ERR_F, .blue = PROS_ERR_F, .brightness = PROS_ERR_F }
+
+optical_rgb_s_t optical_get_rgb(uint8_t port) {
+	optical_rgb_s_t rtn = RGB_ERR_INIT;
+	v5_smart_device_s_t* device;
+	if (!claim_port_try(port - 1, E_DEVICE_OPTICAL)) {
+		return rtn;
+	}
+	device = registry_get_device(port - 1);
+	V5_DeviceOpticalRgb rgb;
+	vexDeviceOpticalRgbGet(device->device_info, &rgb);
+	rtn.red = rgb.red;
+	rtn.green = rgb.green;
+	rtn.blue = rgb.blue;
+	rtn.brightness = rgb.brightness;
+	return_port(port - 1, rtn);
+}
+
+#define RAW_ERR_INIT \
+	{ .clear = PROS_ERR_F, .red = PROS_ERR_F, .green = PROS_ERR_F, .blue = PROS_ERR_F }
+
+optical_raw_s_t optical_get_raw(uint8_t port) {
+	optical_raw_s_t rtn = RGB_ERR_INIT;
+	v5_smart_device_s_t* device;
+	if (!claim_port_try(port - 1, E_DEVICE_OPTICAL)) {
+		return rtn;
+	}
+	device = registry_get_device(port - 1);
+	V5_DeviceOpticalRaw rgb;
+	vexDeviceOpticalRawGet(device->device_info, &rgb);
+	rtn.clear = rgb.clear;
+	rtn.red = rgb.red;
+	rtn.green = rgb.green;
+	rtn.blue = rgb.blue;
+	return_port(port - 1, rtn);
+}
+
+#define GESTURE_ERR_INIT                                                                                    \
+	{                                                                                                         \
+		.udata = PROS_ERR_F, .ddata = PROS_ERR_F, .ldata = PROS_ERR_F, .rdata = PROS_ERR_F, .type = PROS_ERR_F, \
+		.pad = PROS_ERR_F, .count = PROS_ERR_F, .time = PROS_ERR_F                                              \
+	}
+
+int32_t optical_get_gesture(uint8_t port) {
+	claim_port_i(port - 1, E_DEVICE_OPTICAL);
+	V5_DeviceOpticalGesture gesture = GESTURE_ERR_INIT;
+	int32_t rtn = vexDeviceOpticalGestureGet(device->device_info, &gesture);
+	return_port(port - 1, rtn);
+}
+
+void optical_enable_gesture(uint8_t port) {
+	claim_port_i(port - 1, E_DEVICE_OPTICAL);
+	vexDeviceOpticalGestureEnable(device->device_info);
+	return_port(port - 1, 1);
+}
+
+void optical_disable_gesture(uint8_t port) {
+	claim_port_i(port - 1, E_DEVICE_OPTICAL);
+	vexDeviceOpticalGestureEnable(device->device_info);
+	return_port(port - 1, 1);
+}