From c3b516ee1f6e0d317e629ba95abf4ac0cd94f534 Mon Sep 17 00:00:00 2001
From: Clyne Sullivan <tullivan99@gmail.com>
Date: Wed, 10 Feb 2016 14:28:16 -0500
Subject: revised everything
---
src/sensor.c | 88 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 88 insertions(+)
create mode 100644 src/sensor.c
(limited to 'src/sensor.c')
diff --git a/src/sensor.c b/src/sensor.c
new file mode 100644
index 0000000..2725642
--- /dev/null
+++ b/src/sensor.c
@@ -0,0 +1,88 @@
+#include <main.h>
+
+/**
+ * Nowhere else to put this...?
+ */
+
+unsigned char MOTOR_USE_MAP[10] = {
+ 0,0,0,0,0,0,0,0,0,0
+};
+
+int getIMEPort(unsigned int port){
+ switch(port){
+ case CANNON_LEFT : return CANNON_LEFT_IME;
+ case CANNON_RIGHT : return CANNON_RIGHT_IME;
+ case LIFT_PUSHER :
+ case INTAKE_1 :
+ case INTAKE_2 : return -1;
+ case DRIVE_RIGHT : return DRIVE_RIGHT_IME;
+ case DRIVE_LEFT : return DRIVE_LEFT_IME;
+ case LIFT_1 : return LIFT_1_IME;
+ case LIFT_2 : return LIFT_2_IME;
+ case LIFT_ROTATER : return LIFT_ROTATER_IME;
+ }
+ return -1;
+}
+
+int getIME(unsigned int port){
+ int ret;
+ imeGet(port,&ret);
+ return ret;
+}
+
+int getIMEVelocity(unsigned int port){
+ int ret;
+ imeGetVelocity(port,&ret);
+ return ret;
+}
+
+/**
+ * Actual sensor stuffs
+ */
+
+Sensor initSensor(uint32_t port,unsigned char type){
+ Sensor s;
+ s.type = type;
+ switch(s.type){
+ case DIGITAL:
+ pinMode(port,INPUT);
+ s.port = port;
+ //s.initial = digitalRead(port);
+ break;
+ case ANALOG:
+ pinMode(port + 13,INPUT_ANALOG);
+ s.port = port;
+ //s.initial = analogRead(port);
+ break;
+ case GYRO:
+ s.port = 0;
+ s.data.gyro = gyroInit(port,0);
+ //s.initial = gyroGet(s.data.gyro);
+ break;
+ case ULTRASONIC:
+ s.port = 0;
+ s.data.sonic = ultrasonicInit((uint16_t)port,port>>16);
+ //s.initial = ultrasonicGet(s.data.sonic);
+ break;
+ }
+ s.value = 0;
+ return s;
+}
+
+int readSensor(Sensor *s){
+ switch(s->type){
+ case DIGITAL:
+ return (s->value = digitalRead(s->port));
+ break;
+ case ANALOG:
+ return (s->value = analogRead(s->port));
+ break;
+ case GYRO:
+ return (s->value = gyroGet(s->data.gyro));
+ break;
+ case ULTRASONIC:
+ return (s->value = ultrasonicGet(s->data.sonic));
+ break;
+ }
+ return (s->value = -1);
+}
--
cgit v1.2.3