formattign

Author: madhephaestus

ArduinoClassRobot.ino

@@ -26,60 +26,59 @@
 #include <Wire.h>
 #include <EasyBNO055_ESP.h>
 
-
-class Chassis{
+class Chassis {
 public:
-  int lCenter = 86;
-  int rCenter = 87;
-  Servo left;
-  Servo right;
-  double fwdTarget = 0;
-  double rotZTarget = 0;
-  double currentRotationZ=0;
-  double rotZIncrement=1.2;
-  double kp=0.01;
-  double fwdConstant=15;
-  double resetTarget=0;
-  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 = fwdConstant * fwdTarget - 90 * rotZErr + lCenter;
-    rval = -fwdConstant * 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);
-  }
+	int lCenter = 86;
+	int rCenter = 87;
+	Servo left;
+	Servo right;
+	double fwdTarget = 0;
+	double rotZTarget = 0;
+	double currentRotationZ = 0;
+	double rotZIncrement = 1.2;
+	double kp = 0.013;
+	double fwdConstant = 15;
+	double resetTarget = 0;
+	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 = fwdConstant * fwdTarget - 90 * rotZErr + lCenter;
+		rval = -fwdConstant * 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;
-
-
+Chassis puppy;
 
 void otherI2CUpdate() {
 	nunchuck.readData();    // Read inputs and update maps
@@ -96,14 +95,12 @@ float fmap(float x, float in_min, float in_max, float out_min, float out_max) {
 	return (delta * rise) / run + out_min;
 }
 
-
-Chassis puppy;
 // the setup function runs once when you press reset or power the board
 void setup() {
 
 	Serial.begin(115200);
 	Serial.println("Starting ESP32");
-  puppy.begin();
+	puppy.begin();
 	nunchuck.begin();
 	bno.start(&otherI2CUpdate);
 
@@ -114,14 +111,14 @@ 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);
-  puppy.setTargets(x, y,bno.orientationZ);
-  if(nunchuck.values[11]>0){
-    puppy.rotZTarget = puppy.resetTarget;
-  }
-  if(nunchuck.values[10]>0){
-    puppy.resetTarget=puppy.currentRotationZ;
-    puppy.rotZTarget=puppy.currentRotationZ;
-  }
+	puppy.setTargets(x, y, bno.orientationZ);
+	if (nunchuck.values[11] > 0) {
+		puppy.rotZTarget = puppy.resetTarget;
+	}
+	if (nunchuck.values[10] > 0) {
+		puppy.resetTarget = puppy.currentRotationZ;
+		puppy.rotZTarget = puppy.currentRotationZ;
+	}
 	delay(10);
 
 	Serial.print("\n\tx= ");