rename to main.cpp and format for autolib

src/stabilization/stabilization.cpp → src/stabilization/main.cpp

@@ -2,29 +2,18 @@
 #include <Wire.h>
 
 #include "Moti.h"
-#include "ChibiOS_AVR.h"
-#include "Configuration.h"
-#include "Environment.h"
 #include "Color.h"
-#include "Drive.h"
-#include "DriveSystem.h"
-#include "FreeIMU.h"
-#include "Led.h"
+#include "Motion.h"
 #include "Light.h"
-#include "Motor.h"
-#include "Queue.h"
 #include "Sensors.h"
-#include "Serial.h"
-
-/* #include "Vector.h" */
 
 
 void chSetup() {
 	Serial.println(F("Starting..."));
 
-	Drive::start();
-    DriveSystem::start();
-    Environment::start();
+	DriveSystem::start();
+    Motion::start();
+    Moti::start();
     Light::start();
 
 	float currentAngle = 0.0f;
@@ -51,22 +40,22 @@ void chSetup() {
 		*/
 
 		if (abs(output) > 100.0f) {
-			DriveSystem::go(output < 0.0f ? BACKWARD : FORWARD, (uint8_t)abs(output), 100);
+			Motion::go(output < 0.0f ? BACKWARD : FORWARD, (uint8_t)abs(output), 100);
 		}
 		else if (Sensors::getAccY() > 80) {
 			Serial.println(F("SPIN RIGHT"));
-			DriveSystem::spin(RIGHT, 200, 1.57f);
+			Motion::spin(RIGHT, 200, 1.57f);
 		}
 		else if (Sensors::getAccY() < -80) {
 			Serial.println(F("SPIN LEFT"));
-			DriveSystem::spin(LEFT, 200, 1.57f);
+			Motion::spin(LEFT, 200, 1.57f);
 		}
 		else if (abs(currentAngle) > 0.45f) {
 			Serial.println(F("FACING"));
-			DriveSystem::spin(currentAngle > 0.0f ? LEFT : RIGHT, 150, abs(currentAngle));
+			Motion::spin(currentAngle > 0.0f ? LEFT : RIGHT, 150, abs(currentAngle));
 		}
-		else if (DriveSystem::getState() != NONE) {
-			DriveSystem::stop(0);
+		else if (Motion::getState() != NONE) {
+			Motion::stop(0);
 		}
 
 		waitMs(50);