Adding a chassis class to manage orentation

Author: madhephaestus

ArduinoClassRobot.ino

@@ -26,10 +26,59 @@
 #include <Wire.h>
 #include <EasyBNO055_ESP.h>
 
-Servo left;
-Servo right;
+
+class Chassis{
+public:
+  int lCenter = 86;
+  int rCenter = 87;
+  Servo left;
+  Servo right;
+  double fwdTarget = 0;
+  double rotZTarget = 0;
+  double currentRotationZ=0;
+  double rotZIncrement=0.6;
+  double kp=0.01;
+  int lval ;
+  int rval ;
+  Chassis(){}
+  void setTargets(double fwd, double rotz,double currentRotZ){
+    if(abs(rotz)<0.01){
+      rotz=0;
+    }
+    fwdTarget=fwd;
+    rotZTarget+=(rotz*rotZIncrement);
+    currentRotationZ=currentRotZ;
+
+    write();
+  }
+  void begin(){
+    left.attach(33, 1000, 2000);
+    right.attach(32, 1000, 2000);
+    left.write(lCenter);
+    right.write(rCenter);
+  }
+  void write(){
+    double rotZErr = -kp*(rotZTarget-currentRotationZ);
+    lval = 90 * fwdTarget - 90 * rotZErr + lCenter;
+    rval = -90 * fwdTarget - 90 * rotZErr + rCenter;
+    if (lval < 0)
+      lval = 0;
+    if (rval < 0)
+      rval = 0;
+    if (lval > 180)
+      lval = 180;
+    if (rval > 180)
+      rval = 180;
+	  left.write(lval);
+	  right.write(rval);
+  }
+};
+
 Accessory nunchuck;
 EasyBNO055_ESP bno;
+
+
+
 void otherI2CUpdate() {
 	nunchuck.readData();    // Read inputs and update maps
 }
@@ -44,17 +93,15 @@ float fmap(float x, float in_min, float in_max, float out_min, float out_max) {
 	const float delta = x - in_min;
 	return (delta * rise) / run + out_min;
 }
-int lCenter = 86;
-int rCenter = 87;
+
+
+Chassis puppy;
 // the setup function runs once when you press reset or power the board
 void setup() {
 
 	Serial.begin(115200);
 	Serial.println("Starting ESP32");
-	left.attach(33, 1000, 2000);
-	right.attach(32, 1000, 2000);
-	left.write(lCenter);
-	right.write(rCenter);
+  puppy.begin();
 	nunchuck.begin();
 	bno.start(&otherI2CUpdate);
 
@@ -65,31 +112,22 @@ void loop() {
 
 	float x = -fmap(nunchuck.values[1], 0, 255, -1.0, 1.0);
 	float y = -fmap(nunchuck.values[0], 0, 255, -1.0, 1.0);
-	int lval = 90 * x - 90 * y + lCenter;
-	int rval = -90 * x - 90 * y + rCenter;
-	if (lval < 0)
-		lval = 0;
-	if (rval < 0)
-		rval = 0;
-	if (lval > 180)
-		lval = 180;
-	if (rval > 180)
-		rval = 180;
+  puppy.setTargets(x, y,bno.orientationZ);
+  if(nunchuck.values[11]>0){
+    puppy.rotZTarget = 0;
+  }
 
 	delay(10);
 
-	//left.write(lval);
-	//right.write(rval);
-
 	Serial.print("\n\tx= ");
 	Serial.print(bno.orientationX);
 	Serial.print(" |\ty= ");
 	Serial.print(bno.orientationY);
 	Serial.print(" |\tz= ");
 	Serial.print(bno.orientationZ);
 	Serial.print(" |\tL= ");
-	Serial.print(lval);
+	Serial.print(puppy.lval);
 	Serial.print(" |\tR= ");
-	Serial.print(rval);
+	Serial.print(puppy.rval);
 
 }