Cleaned gyro

Gyro.cpp

@@ -81,9 +81,7 @@ void Gyro::startFreeRunningMode(std::function<void(double&, double&)> func) {
 
 	th = new std::thread([this, func](bool* running) {
 		Time stopwatch = Time::stopwatch();
-		double x, vx;
-		double oldx = 0, oldvx = 0;
-		double dt;
+		double vx, oldvx = 0, dt;
 		while(*run) {
 			// wait for new data on the gyro
 			while(!gyroStatusVal&(1<<3)) {
@@ -106,19 +104,13 @@ void Gyro::startFreeRunningMode(std::function<void(double&, double&)> func) {
 			vx-=xOff;
 
 			vx = vx*0.3 + oldvx*0.7;
-
-			//if(abs(oldvx-vx)<1)
-			//	vx = oldvx;
+			oldvx = vx;
 
 			dt = stopwatch.elapsed_ns() * 1e-9;
-			x = oldx + vx * dt * 1e3;
 
-			func(x, dt);
+			func(vx, dt);
 
 			stopwatch.reset();
-
-			oldx = x;
-			oldvx = vx;
 		}
 	}, std::ref(run));
 	th->detach();

Log.cpp

@@ -6,6 +6,10 @@ Log::Log(std::string filename, char sep) : sep(sep) {
 	file.open("/home/"+filename);
 }
 
+void Log::println(long time, double sp) {
+	file << time << sep << sp << std::endl;
+}
+
 void Log::println(long time, double sp, double csp) {
 	file << time << sep << sp << sep << csp << std::endl;
 }

Log.h

@@ -8,6 +8,7 @@ class Log {
 public:
 	Log(std::string filename, char sep = ' ');
 	~Log();
+	void println(long time, double angle);
 	void println(long time, double angle, double pidOutput);
 	void println(long time, double sp, double csp, double spd);
 	void println(long time, double sp, double csp, long enc, long filtered);

Wifi.cpp

@@ -68,7 +68,7 @@ void Wifi::openServer() {
 	} while(true);
 }
 
-void Wifi::updateAngle(short& angle) {
+void Wifi::updateAngle(short angle) {
 	if(connected)
 		writeShort(angle);
 }

Wifi.h

@@ -18,7 +18,7 @@ namespace myRIO {
 
 		void openServer();
 		bool isConnected();
-		void updateAngle(short&angle);
+		void updateAngle(short angle);
 		void updateEncoder(uint8_t setpoint, uint32_t enc);
 
 	private:

main.cpp

@@ -5,8 +5,8 @@
 #include <math.h>
 
 //#define DEF_WIFI
-//#define DEF_MOTOR
-#define DEF_I2C
+#define DEF_MOTOR
+//#define DEF_I2C
 //#define DEF_PENDULUM
 
 using namespace std;
@@ -61,7 +61,7 @@ int main() {
 
 	// GYROSCOPE
 	Gyro gyro;
-	double xRotOff, angle;
+	double angle = 180;
 	// initializing and calibrating the gyro
 	if(myRIO_error()) {cout << "Gyro - Error while initializing" << endl; return -1;}
 	gyro.calibrate();
@@ -70,12 +70,13 @@ int main() {
 	DIO::writeLed(LED0, HIGH);
 
 	// COMPLEMENTARY FILTER
-	const double GYRO_WEIGHT = 0.75;
+	const double GYRO_WEIGHT = 0.7;
 
 #ifdef DEF_PENDULUM
 	// PENDULUM
 	PendulumPID pendulumPid(1, 0.01, 0.01);
-	pendulumPid.setSetpoint(90);
+	double pendulumSetpoint = 90;
+	pendulumPid.setSetpoint(pendulumSetpoint);
 	double motorSpeed;
 
 	// COUNTDOWN
@@ -91,9 +92,18 @@ int main() {
 	DIO::writeLed(LED2, LOW);
 	DIO::writeLed(LED3, LOW);
 #endif
+#ifdef DEF_WIFI
+	Wifi w([&](short setpoint) {
+#ifdef DEF_PENDULUM
+		pendulumPid.setSetpoint(pendulumSetpoint);
+#endif
+		cout << "Setpoint = " << setpoint << endl;
+	});
+	while(!w.isConnected());
+#endif
 
 	// starting the angle thread
-	gyro.startFreeRunningMode([&](double &xRot, double &dt){
+	gyro.startFreeRunningMode([&](double &vxRot, double &dt){
 		system("clear");
 
 		// reading accelerometer values
@@ -108,17 +118,13 @@ int main() {
 		if(xAcc<90) xAcc = map(xAcc, 0, 78.894271, 0, 90);
 		else xAcc = map(xAcc, 103.148749, 180, 90, 180);
 
-
-		// gyro angle at 180° will be  from calibration : offset that
-		xRotOff = 180. + xRot;
-
 		// filter accelerometer data
 		accFiltered = accFilterA*oldAcc + accFilterB*oldAccFiltered;
 		oldAccFiltered = accFiltered;
 		oldAcc = xAcc;
 
 		// complementary filter
-		angle = GYRO_WEIGHT*xRotOff + (1.-GYRO_WEIGHT)*accFiltered;
+		angle = GYRO_WEIGHT*(angle + vxRot * dt) + (1.-GYRO_WEIGHT)*accFiltered;
 
 
 #ifdef DEF_PENDULUM
@@ -132,18 +138,18 @@ int main() {
 		printf("dt = %ld, xRot = %f, xAcc = %f, angle = %f, motorSpeed = %f\n", (unsigned long)dt*1e9, xRotOff, xAcc, angle, motorSpeed);
 		logA.println(dt*1e9, angle, motorSpeed);
 #else
-		cout << "dt " << dt*1e9 << " xRot " << xRotOff << " xAcc " << xAcc << " angle " << angle << endl;
-		logA.println(dt*1e9, xRotOff, xAcc, angle);
-#endif
-	});
+		cout << "dt " << dt*1e9 << " xAcc " << xAcc << " angle " << angle << endl;
+		logA.println(dt*1e9, xAcc, angle);
 #endif
 #ifdef DEF_WIFI
-	Wifi w([&](short setpoint) {});
-	while(!w.isConnected());
+		w.updateAngle((short)angle);
+#endif
+	});
 #endif
 
-	while(1);
-
+	motorLeftPid.setSetpoint(360);
+	motorLeft.setSpeed(10);
+	Time::wait_s(2);
 
 #ifdef DEF_I2C
 	logA.close();