From 9b894a9bb16fdc0c18dab5c4b38e55cb104d3619 Mon Sep 17 00:00:00 2001
From: Lukas <lukas.s.kaul@gmail.com>
Date: Sun, 10 Mar 2019 19:55:38 +0100
Subject: [PATCH] added control mode setters (current and velocity)

---
 Arduino/ODriveArduino/ODriveArduino.cpp | 8 ++++++++
 Arduino/ODriveArduino/ODriveArduino.h   | 2 ++
 2 files changed, 10 insertions(+)

diff --git a/Arduino/ODriveArduino/ODriveArduino.cpp b/Arduino/ODriveArduino/ODriveArduino.cpp
index 00fce19a..f9d4a356 100644
--- a/Arduino/ODriveArduino/ODriveArduino.cpp
+++ b/Arduino/ODriveArduino/ODriveArduino.cpp
@@ -46,6 +46,14 @@ void ODriveArduino::TrapezoidalMove(int motor_number, float position){
     serial_ << "t " << motor_number << " " << position << "\n";
 }
 
+void ODriveArduino::SetControlModeCurrent(int motor_number){
+    serial_<< "w axis" << motor_number << ".controller.config.control_mode " << 1 << "\n";
+}
+
+void ODriveArduino::SetControlModeVelocity(int motor_number){
+    serial_<< "w axis" << motor_number << ".controller.config.control_mode " << 2 << "\n";
+}
+
 float ODriveArduino::readFloat() {
     return readString().toFloat();
 }
diff --git a/Arduino/ODriveArduino/ODriveArduino.h b/Arduino/ODriveArduino/ODriveArduino.h
index 86b3aaf1..ad0e922b 100644
--- a/Arduino/ODriveArduino/ODriveArduino.h
+++ b/Arduino/ODriveArduino/ODriveArduino.h
@@ -28,6 +28,8 @@ public:
     void SetVelocity(int motor_number, float velocity, float current_feedforward);
     void SetCurrent(int motor_number, float current);
     void TrapezoidalMove(int motor_number, float position);
+	void SetControlModeCurrent(int motor_number);
+    void SetControlModeVelocity(int motor_number);
     // General params
     float readFloat();
     int32_t readInt();