Diff: LamborSteer.cpp

Revision:

0:b20808175db0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LamborSteer.cpp	Wed Nov 07 16:51:16 2018 +0000
@@ -0,0 +1,183 @@
+#include "LamborSteer.h"
+#include "mbed.h"
+
+LamborSteer::LamborSteer(PinName _cw_pin, PinName _ccw_pin, PinName _enablerPin, PinName _sensor_pin, int _offset)
+: cw_pin(_cw_pin), ccw_pin(_ccw_pin), enabler_pin(_enablerPin), sensor_pin(_sensor_pin)
+{
+  if (s_index == MAX_SERVOS)
+  {
+    return;
+  }
+  
+  index = s_index;
+  
+  _m_servos[s_index++] = this;
+
+  tolerance = 15;
+  
+  /* New */
+  offset = _offset;
+  active = true;
+  
+  cw_pin = 0;
+  ccw_pin = 0;
+}
+
+// timer interrupt routine
+void LamborSteer::timer_int_routine()         
+{
+  for (int i = 0; i < s_index; i++)
+  {
+    if (_m_servos[i]->active)
+    {
+      _m_servos[i]->update();
+    }
+  }
+}
+
+
+void LamborSteer::detach()
+{
+  stop();
+  
+  active = false;
+}
+
+void LamborSteer::write(int degrees)
+{
+  // map degrees to potentiometer value
+  int pot_value = map(degrees, MIN_ANGLE, MAX_ANGLE, MIN_POT_VALUE, MAX_POT_VALUE);  
+
+  write_pot_value(pot_value);
+}
+
+void LamborSteer::update()
+{
+  int current_position = get_position();
+
+  // Guard servo's min and max bounds
+  if (current_position < MIN_POT_VALUE)
+  {
+    write(MIN_ANGLE);
+  }
+
+  if (current_position > MAX_POT_VALUE)
+  {
+    write(MAX_ANGLE);
+  }
+
+  // Gap between curent and wanted positions
+  int gap = abs(current_position - wanted_position);
+
+  // If gap is within tolerance then stop
+  if (gap < tolerance)
+  {
+    stop();
+
+    return;
+  }
+
+  // set speed according to gap remaining 
+  speed = 100;    //125
+
+  if (gap > 50)
+  {
+    speed = 100;  //165
+  }
+  if (gap > 75)
+  {
+    speed = 100;  //205
+  }
+  if (gap > 100)
+  {
+    speed = 100;  //255
+  }
+
+  // Determine direction
+  if (current_position < wanted_position)
+  {
+    run_cw();
+  }
+
+  if (current_position > wanted_position)
+  {
+    run_ccw();
+  }
+}
+
+servo_status LamborSteer::read()
+{
+  // Get current postion, wanted position and speed
+  servo_status current_status;
+  
+  int current_angle = map(get_position(), MIN_POT_VALUE, MAX_POT_VALUE, MIN_ANGLE, MAX_ANGLE);
+  int wanted_angle = map(wanted_position, MIN_POT_VALUE, MAX_POT_VALUE, MIN_ANGLE, MAX_ANGLE);
+
+  current_status.current_angle = current_angle;
+  current_status.wanted_angle = wanted_angle;
+  current_status.speed = speed;
+
+  return current_status;
+}
+
+/*************** Private stuff **************/
+
+void LamborSteer::write_pot_value(int value)
+{
+  // Constrain value to servo's min and max bounds
+  //int constrained_value = constrain(value, MIN_POT_VALUE + tolerance, MAX_POT_VALUE - tolerance);
+  if(value > MAX_POT_VALUE - tolerance)
+  {
+    constrained_value = MAX_POT_VALUE - tolerance;
+  }
+  else if(value < MIN_POT_VALUE + tolerance)
+  {
+    constrained_value = MIN_POT_VALUE + tolerance;  
+  }
+  else
+  {
+    constrained_value = value;   
+  }
+  // set destination
+  wanted_position = constrained_value;
+}
+
+// Run motor clockwise at current speed
+void LamborSteer::run_cw()
+{
+  cw_pin = 1;
+  ccw_pin = 0;
+
+  enabler_pin.write(speed / 255.0f);
+}
+
+// Run motor counter clockwise at current speed
+void LamborSteer::run_ccw()
+{
+  cw_pin = 0;
+  ccw_pin = 1;
+
+  enabler_pin.write(speed / 255.0f);
+}
+
+// Stop the motor, set speed to 0
+void LamborSteer::stop()
+{
+  cw_pin = 0;
+  ccw_pin = 0;
+  
+  enabler_pin.write(0);
+}
+
+// Get current offset potentiometer position
+int LamborSteer::get_position()
+{
+  return (sensor_pin.read()*1023) + offset;  // Read the analog input value (value from 0.0 to 1.0)
+                                             // And convert to 0 - 1023
+}
+
+// Mapping
+long LamborSteer::map(long x, long in_min, long in_max, long out_min, long out_max)
+{
+  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
\ No newline at end of file