Diff: motor.cpp

Revision:

10:d50c4957a193

Parent:

9:48a0b4a67283

Child:

11:34bd7f42c9db

--- a/motor.cpp	Wed Oct 31 09:12:28 2018 +0000
+++ b/motor.cpp	Wed Oct 31 10:38:38 2018 +0000
@@ -55,9 +55,10 @@
     printcount = 0;
 }
 
-void Motor::set_current_angle_as_zero() {
+void Motor::define_current_angle_as_x_radians(double radians)() {
     encoder.reset();
     target_angle = 0;
+    rotation_frame_offset = radians;
 }
 
 void Motor::set_pid_k_values(double k_p, double k_i, double k_d) {
@@ -69,11 +70,13 @@
 }
 
 void Motor::set_target_angle(double angle) {
-    target_angle = angle / extra_reduction_ratio;
+    // Preform calculation between motor angles and robot angles.
+    target_angle = (angle - rotation_frame_offset) / extra_reduction_ratio;
 }
 
 double Motor::get_current_angle() {
-    return encoder_pulses_to_radians(encoder.getPulses()) * extra_reduction_ratio;
+    // Preform calculation between motor angles and robot angles.
+    return (encoder_pulses_to_radians(encoder.getPulses()) * extra_reduction_ratio) + rotation_frame_offset;
 }
 
 void Motor::update() {