linear_actuator_p_control.ino

// Software Serial Sample
// Copyright (c) 2012 Dimension Engineering LLC
// See license.txt for license details.

#include <SoftwareSerial.h>
#include <SabertoothSimplified.h>

SoftwareSerial SWSerial(NOT_A_PIN, 11); // RX on no pin (unused), TX on pin 11 (to S1).
SabertoothSimplified ST(SWSerial); // Use SWSerial as the serial port.

double Kp = 0.5;

void setup()
{
  Serial.begin(9600);
  SWSerial.begin(9600);
  Serial.setTimeout(50);
}

void loop()
{
   if (Serial.available() > 0) {
      double target_pos = Serial.parseFloat() ; // constrain(Serial.parseFloat(), 100.0, 1023.0);
      if ( target_pos < 100 )
        target_pos = 100;
      else if ( target_pos > 1023 )
        target_pos = 1023;
        
      Serial.println(target_pos);
      
      int output = 0;
  
      do
      {
        // Simple P controller
        double pos = double(analogRead(A0));
        
        Serial.print("pos: ");
        Serial.print(pos);
        
        output = int(Kp * (pos-target_pos));
        
        output = constrain(output, -127, 127);
        Serial.print(", output: ");
        Serial.println(output);
        
        ST.motor(1, output);
      } while (abs(output) > 10);
   }
}