add alignment and cogging test

examples/utils/alignment_and_cogging_test/alignment_and_cogging_test.ino

@@ -0,0 +1,113 @@
+#include <Arduino.h>
+#include <SimpleFOC.h>
+
+BLDCMotor motor = BLDCMotor(9, 10, 11, 7);
+MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0x0E, 4);
+
+
+/**
+ * This measures how closely sensor and electrical angle agree and how much your motor is affected by 'cogging'.  
+ * It can be used to investigate how much non linearity there is between what we set (electrical angle) and what we read (sensor angle)
+ * This non linearity could be down to magnet placement, coil winding differences or simply that the magnetic field when travelling through a pole pair is not linear
+ * An alignment error of ~10 degrees and cogging of ~4 degrees is normal for small gimbal.
+ * The following article is an interesting read
+ * https://hackaday.com/2016/02/23/anti-cogging-algorithm-brings-out-the-best-in-your-hobby-brushless-motors/
+ */
+void testAlignmentAndCogging(int direction) {
+
+  motor.setPhaseVoltage(motor.voltage_sensor_align, 0);
+
+  _delay(200);
+
+  float initialAngle = sensor.getAngle();
+
+  const int shaft_rotation = 720; // 720 deg test - useful to see repeating cog pattern
+  int sample_count = int(shaft_rotation * motor.pole_pairs); // test every electrical degree
+
+  float stDevSum = 0;
+
+  float mean = 0.0;
+  float prev_mean = 0.0;
+
+
+  for (int i = 0; i < sample_count; i++) {
+
+    float electricAngle = (float) direction * i * motor.pole_pairs * shaft_rotation / sample_count;
+    // move and wait
+    motor.setPhaseVoltage(motor.voltage_sensor_align, electricAngle * PI / 180);
+    _delay(5);
+
+    // measure 
+    float sensorAngle = (sensor.getAngle() - initialAngle) * 180 / PI;
+    float sensorElectricAngle = sensorAngle * motor.pole_pairs;
+    float electricAngleError = electricAngle - sensorElectricAngle;
+
+    // plot this - especially electricAngleError
+    Serial.print(electricAngle);
+    Serial.print("\t");
+    Serial.print(sensorElectricAngle );
+    Serial.print("\t");
+    Serial.println(electricAngleError);
+
+    // use knuth standard deviation algorithm so that we don't need an array too big for an Uno
+    prev_mean = mean;
+    mean = mean + (electricAngleError-mean)/(i+1);
+    stDevSum = stDevSum + (electricAngleError-mean)*(electricAngleError-prev_mean);
+
+  }
+
+  Serial.println();
+  Serial.println("ALIGNMENT AND COGGING REPORT");
+  Serial.println();
+  Serial.print("Direction: ");
+  Serial.println(direction);
+  Serial.print("Mean error (alignment): ");
+  Serial.print(mean);
+  Serial.println(" deg (electrical)");
+  Serial.print("Standard Deviation (cogging): ");
+  Serial.print(sqrt(stDevSum/sample_count));
+  Serial.println(" deg (electrical)");
+  Serial.println();
+  Serial.println("Plotting 3rd column of data (electricAngleError) will likely show sinusoidal cogging pattern with a frequency of 4xpole_pairs per rotation");
+  Serial.println();
+
+}
+
+void setup() {
+
+  Serial.begin(115200);
+  while (!Serial) ;
+
+  motor.voltage_power_supply = 9;
+  motor.voltage_sensor_align = 3;
+  motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
+  motor.controller = ControlType::velocity;
+
+  sensor.init();
+  motor.linkSensor(&sensor);
+
+  motor.useMonitoring(Serial);
+  motor.init();
+  motor.initFOC();
+
+  testAlignmentAndCogging(1);
+
+  motor.setPhaseVoltage(0, 0);
+  Serial.println("Press any key to test in CCW direction");
+  while (!Serial.available()) { }
+
+  testAlignmentAndCogging(-1);
+
+  Serial.println("Complete");
+
+  motor.setPhaseVoltage(0, 0);
+  while (true) ; //do nothing;
+
+}
+
+
+
+
+void loop() {
+
+}