From 60a2f2bcaaefd872158fca5caf454a9ad4cfcbe3 Mon Sep 17 00:00:00 2001
From: 0xFEEDC0DE64 <daniel@brunner.ninja>
Date: Sun, 16 May 2021 22:47:35 +0200
Subject: [PATCH] More implementations

---
 platformio.ini | 28 ++++++++++++++--------------
 src/can.h      | 40 ++++++++++++++++++++++++++++++++++++++++
 2 files changed, 54 insertions(+), 14 deletions(-)

diff --git a/platformio.ini b/platformio.ini
index ad50e9e..58a3c15 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -130,20 +130,20 @@ build_flags =
   ${webserver_common.build_flags}
 ;  -DFEATURE_ARDUINOOTA
 ;  -DFEATURE_WEBOTA
-  -DFEATURE_DPAD_5WIRESW
-  -DPINS_DPAD_5WIRESW_OUT=32
-  -DPINS_DPAD_5WIRESW_IN1=25
-  -DPINS_DPAD_5WIRESW_IN2=26
-  -DPINS_DPAD_5WIRESW_IN3=27
-  -DPINS_DPAD_5WIRESW_IN4=21
-  -DDPAD_5WIRESW_UP=4
-  -DDPAD_5WIRESW_DOWN=3
-  -DDPAD_5WIRESW_CONFIRM=7
-  -DDPAD_5WIRESW_BACK=0
-  -DDPAD_5WIRESW_PROFILE0=1
-  -DDPAD_5WIRESW_PROFILE1=5
-  -DDPAD_5WIRESW_PROFILE2=2
-  -DDPAD_5WIRESW_PROFILE3=6
+;  -DFEATURE_DPAD_5WIRESW
+;  -DPINS_DPAD_5WIRESW_OUT=32
+;  -DPINS_DPAD_5WIRESW_IN1=25
+;  -DPINS_DPAD_5WIRESW_IN2=26
+;  -DPINS_DPAD_5WIRESW_IN3=27
+;  -DPINS_DPAD_5WIRESW_IN4=21
+;  -DDPAD_5WIRESW_UP=4
+;  -DDPAD_5WIRESW_DOWN=3
+;  -DDPAD_5WIRESW_CONFIRM=7
+;  -DDPAD_5WIRESW_BACK=0
+;  -DDPAD_5WIRESW_PROFILE0=1
+;  -DDPAD_5WIRESW_PROFILE1=5
+;  -DDPAD_5WIRESW_PROFILE2=2
+;  -DDPAD_5WIRESW_PROFILE3=6
 ;  -DDPAD_5WIRESW_DEBUG
   -DDEFAULT_GASMIN=750
   -DDEFAULT_GASMAX=3700
diff --git a/src/can.h b/src/can.h
index a840467..d453b87 100644
--- a/src/can.h
+++ b/src/can.h
@@ -170,6 +170,46 @@ void parseCanInput()
         controllers.front.feedbackValid = true;
         controllers.front.feedback.right.current = *((int16_t*)message.data);
         break;
+    case MotorControllerFrontLeftSpeed:
+        m_lastCanFeedback = millis();
+        controllers.front.feedbackValid = true;
+        controllers.front.feedback.left.speed = *((int16_t*)message.data);
+        break;
+    case MotorControllerFrontRightSpeed:
+        m_lastCanFeedback = millis();
+        controllers.front.feedbackValid = true;
+        controllers.front.feedback.right.speed = *((int16_t*)message.data);
+        break;
+    case MotorControllerFrontLeftError:
+        m_lastCanFeedback = millis();
+        controllers.front.feedbackValid = true;
+        controllers.front.feedback.left.error = *((int8_t*)message.data);
+        break;
+    case MotorControllerFrontRightError:
+        m_lastCanFeedback = millis();
+        controllers.front.feedbackValid = true;
+        controllers.front.feedback.right.error = *((int8_t*)message.data);
+        break;
+    case MotorControllerFrontLeftAngle:
+        m_lastCanFeedback = millis();
+        controllers.front.feedbackValid = true;
+        controllers.front.feedback.left.angle = *((int16_t*)message.data);
+        break;
+    case MotorControllerFrontRightAngle:
+        m_lastCanFeedback = millis();
+        controllers.front.feedbackValid = true;
+        controllers.front.feedback.right.angle = *((int16_t*)message.data);
+        break;
+    case MotorControllerFrontLeftChops:
+        m_lastCanFeedback = millis();
+        controllers.front.feedbackValid = true;
+        controllers.front.feedback.left.chops = *((uint16_t*)message.data);
+        break;
+    case MotorControllerFrontRightChops:
+        m_lastCanFeedback = millis();
+        controllers.front.feedbackValid = true;
+        controllers.front.feedback.right.chops = *((uint16_t*)message.data);
+        break;
     case MotorControllerFrontLeftVoltage:
         m_lastCanFeedback = millis();
         controllers.front.feedbackValid = true;