Add good version

RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_SERVO.step/R2Home_SERVO.step.ino

@@ -0,0 +1,83 @@
+#include <Servo.h>
+#include <ServoInput.h>
+
+const int InputPinServo = 2;
+const int InputPinCmd = 10;
+const int OutputPin = 3; 
+
+ServoInputPin<InputPinServo> inputServo;
+ServoInputPin<InputPinCmd> inputCmd;
+
+Servo outputServo;
+
+float parallax_steer = 1500; 
+float feedback_value = 1500; 
+
+unsigned long tread = 0; 
+unsigned long tparallax = 0; 
+
+float Kp = 1.5;
+float Kd = 3;
+float cumerror = 0; 
+float lasterror = 0;  
+
+float error = 0; 
+float PIDsum = 0; 
+float cmd = 0; 
+float raterror = 0; 
+
+
+void setup() {
+ 
+ Serial.begin(57600); 
+ outputServo.attach(OutputPin);
+
+}
+
+void loop() {
+  
+  getcmd(50); 
+  updatecmd(200); 
+
+
+}
+
+void getcmd(int a) { 
+
+if ((millis()-tread)>=(1000/a)) {
+
+  tread = millis(); 
+
+  parallax_steer = inputCmd.getPulse();
+  parallax_steer = map(parallax_steer,1000, 2000, 1050, 1950);   
+
+  Serial.print(parallax_steer); Serial.print(","); Serial.print(feedback_value); Serial.print(","); Serial.print(error);  Serial.print(",");  Serial.println(PIDsum);  
+ 
+  }
+}
+
+void updatecmd(int a) { 
+  
+  if ((millis()-tparallax)>=(1000/a)) { 
+
+      tparallax = millis(); 
+      
+      feedback_value = inputServo.getPulseRaw();
+      
+      if ((feedback_value <= 0) or (feedback_value >= 2000)) { feedback_value = 500; }
+      
+      feedback_value = map(feedback_value, 0, 1000, 2000, 1000); 
+   
+      error = (parallax_steer - feedback_value); 
+      raterror = (error-lasterror);
+      PIDsum = (Kp*error)+(Kd*raterror);  
+      cmd = 1500 + PIDsum; 
+
+      constrain(cmd, 1000, 2000); 
+ 
+      outputServo.writeMicroseconds(cmd);
+      
+      lasterror = error; 
+    
+  } 
+}

RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_SERVO_V0.01/R2Home_SERVO_V0.01.ino

@@ -1,80 +0,0 @@
-#include <Servo.h>
-#include <movingAvg.h>
-
-movingAvg st(5); 
-Servo steer;
-
-#define feedback_pin 2
-#define cmd_pin 10
-
-float parallax_steer = 1500; 
-float feedback_value = 0;
-
-const float Kp = 0.8;
-const float Kd = 0.03;
-float cumerror = 0; 
-float lasterror = 0;  
-
-unsigned long tparallax = 0; 
-unsigned long tread = 0; 
-
-void setup() {
-
-  st.begin();
-  steer.attach(3, 1000, 2000);
-
-}
-
-void loop() {
-
-  Serial.begin(57600); 
-
-  getcmd(50); 
-  updatecmd(200); 
-
-}
-
-void getcmd(int a) {
-
- if ((millis()-tread)>=(1000/a)) { 
-  parallax_steer = pulseIn(10, HIGH, 2100); 
- }
-
- if ((parallax_steer <= 999) or (parallax_steer >= 2001)) { 
-  parallax_steer = 1500; 
- }
-
- Serial.println(parallax_steer); 
-
-}
-
- 
-void updatecmd(int a) { 
-  
-  if ((millis()-tparallax)>=(1000/a)) { 
-
-      tparallax = millis(); 
-      
-      feedback_value = (((pulseIn(feedback_pin, HIGH, 1200))-30)/1.050); 
-
-      //Serial.println(feedback_value); 
-      
-      if ((feedback_value < 0) or (feedback_value > 2000)) { feedback_value = 500; }
-      
-      feedback_value = st.reading(feedback_value); 
-      feedback_value = map(feedback_value, 0, 1000, 1000, 2000); 
-   
-      float error = (parallax_steer - feedback_value); 
-      float raterror = ((error-lasterror)/(1/a));
-      float PIDsum = ((Kp*error)+(Kd*raterror)); 
-      float cmd = 1500 + PIDsum; 
-
-      constrain(cmd, 1000, 2000); 
-      cmd = map(cmd, 1000, 2000, 85,95);
-       
-      lasterror = error; 
-    
-      steer.write(cmd);
-    
-  } 
-}