Dev

examples/encoders/calibrated/sensor_calibration.ino

@@ -0,0 +1,84 @@
+/**
+ * Torque control example using voltage control loop.
+ * 
+ * Most of the low-end BLDC driver boards doesn't have current measurement therefore SimpleFOC offers 
+ * you a way to control motor torque by setting the voltage to the motor instead hte current. 
+ * 
+ * This makes the BLDC motor effectively a DC motor, and you can use it in a same way.
+ */
+#include <SimpleFOC.h>
+
+// magnetic sensor instance - SPI
+MagneticSensorSPI sensor = MagneticSensorSPI(AS5048_SPI, PB6);
+// BLDC motor & driver instance
+BLDCMotor motor = BLDCMotor(11);
+BLDCDriver3PWM driver = BLDCDriver3PWM(PB4,PC7,PB10,PA9);
+// instantiate the calibrated sensor object
+CalibratedSensor sensor_calibrated = CalibratedSensor(sensor);
+
+// voltage set point variable
+float target_voltage = 2;
+
+// instantiate the commander
+Commander command = Commander(Serial);
+
+void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); }
+
+void setup() {
+
+  SPI.setMISO(PB14);
+  SPI.setMOSI(PB15);
+  SPI.setSCLK(PB13);
+
+  sensor.init();
+  // Link motor to sensor
+  motor.linkSensor(&sensor);
+  // power supply voltage
+  driver.voltage_power_supply = 20;
+  driver.init();
+  motor.linkDriver(&driver);
+  // aligning voltage 
+  motor.voltage_sensor_align = 8;
+  motor.voltage_limit = 20;
+  // set motion control loop to be used
+  motor.controller = MotionControlType::torque;
+
+  // use monitoring with serial 
+  Serial.begin(115200);
+  // comment out if not needed
+  motor.useMonitoring(Serial);
+  motor.monitor_variables =  _MON_VEL; 
+  motor.monitor_downsample = 10; // default 10
+
+  // initialize motor
+  motor.init();
+
+  // set voltage to run calibration
+  sensor_calibrated.voltage_calibration = 6;
+  // Running calibration
+  sensor_calibrated.calibrate(motor); 
+
+  //Serial.println("Calibrating Sensor Done.");
+  // Linking sensor to motor object
+  motor.linkSensor(&sensor_calibrated);
+
+  // calibrated init FOC
+  motor.initFOC();
+
+  // add target command T
+  command.add('T', doTarget, "target voltage");
+
+  Serial.println(F("Motor ready."));
+
+  Serial.println(F("Set the target voltage using serial terminal:"));
+  _delay(1000);
+}
+
+void loop() {
+
+  motor.loopFOC();
+  motor.move(target_voltage);
+  command.run();
+  motor.monitor();
+
+}

src/encoders/calibrated/CalibratedSensor.cpp

@@ -0,0 +1,257 @@
+#include "CalibratedSensor.h"
+
+
+// CalibratedSensor()
+// sensor              - instance of original sensor object
+CalibratedSensor::CalibratedSensor(Sensor& wrapped) : _wrapped(wrapped) 
+{
+};
+
+
+CalibratedSensor::~CalibratedSensor()
+{
+	delete calibrationLut;
+};
+
+// call update of calibrated sensor
+void CalibratedSensor::update(){
+    _wrapped.update();
+    this->Sensor::update();
+};
+
+// Retrieve the calibrated sensor angle
+void CalibratedSensor::init() {
+    // assume wrapped sensor has already been initialized
+    this->Sensor::init(); // call superclass init
+}
+
+// Retrieve the calibrated sensor angle
+float CalibratedSensor::getSensorAngle(){
+    // raw encoder position e.g. 0-2PI
+    float rawAngle = _wrapped.getMechanicalAngle();
+
+    // index of the bucket that rawAngle is part of. 
+    // e.g. rawAngle = 0 --> bucketIndex = 0.
+    // e.g. rawAngle = 2PI --> bucketIndex = 128.
+    int bucketIndex = floor(rawAngle/(_2PI/n_lut));
+    float remainder = rawAngle - ((_2PI/n_lut)*bucketIndex);
+
+    // Extract the lower and upper LUT value in counts
+    float y0 = calibrationLut[bucketIndex]; 
+    float y1 = calibrationLut[(bucketIndex+1)%n_lut]; 
+
+    // Linear Interpolation Between LUT values y0 and y1 using the remainder
+    // If remainder = 0, interpolated offset = y0
+    // If remainder = 2PI/n_lut, interpolated offset = y1
+    float interpolatedOffset = (((_2PI/n_lut)-remainder)/(_2PI/n_lut))*y0 + (remainder/(_2PI/n_lut))*y1; 
+
+    // add offset to the raw sensor count. Divide multiply by 2PI/CPR to get radians
+    float calibratedAngle = rawAngle+interpolatedOffset; 
+
+    // return calibrated angle in radians
+    return calibratedAngle;
+}
+
+void CalibratedSensor::calibrate(BLDCMotor& motor){
+
+    Serial.println("Starting Sensor Calibration.");
+
+    int _NPP = motor.pole_pairs;                                    // number of pole pairs which is user input
+    const int n_ticks = 128*_NPP;                                   // number of positions to be sampled per mechanical rotation.  Multiple of NPP for filtering reasons (see later)
+    const int n2_ticks = 40;                                        // increments between saved samples (for smoothing motion)
+    float deltaElectricalAngle = _2PI*_NPP/(n_ticks*n2_ticks);      // Electrical Angle increments for calibration steps    
+    float* error_f  = new float[n_ticks]();                         // pointer to error array rotating forwards
+  //  float* raw_f = new float[n_ticks]();                            // pointer to raw forward position
+    float* error_b  = new float[n_ticks]();                         // pointer to error array rotating forwards
+  //  float* raw_b = new float[n_ticks]();                            // pointer to raw backword position
+    float* error = new float[n_ticks]();                            // pointer to error array (average of forward & backward)
+    float* error_filt = new float[n_ticks]();                       // pointer to filtered error array (low pass filter)
+    const int window = 128;                                         // window size for moving average filter of raw error
+    motor.zero_electric_angle = 0;                                  // Set position sensor offset
+
+    // find natural direction (this is copy of the init code)
+    // move one electrical revolution forward
+    for (int i = 0; i <=500; i++ ) {
+      float angle = _3PI_2 + _2PI * i / 500.0f;
+      motor.setPhaseVoltage(voltage_calibration, 0,  angle);
+        _wrapped.update();
+      _delay(2);
+    }
+    // take and angle in the middle
+    _wrapped.update();
+    float mid_angle = _wrapped.getAngle();
+    // move one electrical revolution backwards
+    for (int i = 500; i >=0; i-- ) {
+      float angle = _3PI_2 + _2PI * i / 500.0f ;
+      motor.setPhaseVoltage(voltage_calibration, 0,  angle);
+        _wrapped.update();
+      _delay(2);
+    }
+    _wrapped.update();
+    float end_angle = _wrapped.getAngle();
+    motor.setPhaseVoltage(0, 0, 0);
+    _delay(200);
+    // determine the direction the sensor moved
+    int directionSensor;
+    if (mid_angle < end_angle) {
+      Serial.println("MOT: sensor_direction==CCW");
+      directionSensor = -1;
+      motor.sensor_direction = Direction::CCW;
+
+    } else{
+      Serial.println("MOT: sensor_direction==CW");
+      directionSensor = 1;
+      motor.sensor_direction = Direction::CW;
+
+    }
+
+    //Set voltage angle to zero, wait for rotor position to settle
+    // keep the motor in position while getting the initial positions
+    motor.setPhaseVoltage(voltage_calibration, 0, elec_angle);
+    _delay(1000);
+    _wrapped.update();
+    float theta_init = _wrapped.getAngle();
+    float theta_absolute_init = _wrapped.getMechanicalAngle();
+
+    /* 
+    Start Calibration
+    Loop over  electrical angles from 0 to NPP*2PI, once forward, once backward
+    store actual position and error as compared to electrical angle
+    */
+
+	/* 
+	forwards rotation
+	*/
+	Serial.println("Rotating forwards");
+	int k = 0;
+	for(int i = 0; i<n_ticks; i++)
+	{                                                 
+		for(int j = 0; j<n2_ticks; j++)
+			{   
+				elec_angle += deltaElectricalAngle;
+				motor.setPhaseVoltage(voltage_calibration, 0, elec_angle);
+			}
+
+		// delay to settle in position before taking a position sample
+		_delay(20);
+		_wrapped.update();
+		theta_actual = _wrapped.getAngle()-theta_init;
+		if (directionSensor == -1)
+		{
+			theta_actual = -theta_actual;
+			error_f[i] = theta_actual - elec_angle/_NPP;
+
+		}
+		else
+		{
+			error_f[i] = elec_angle/_NPP - theta_actual;
+		}
+		// if overflow happened track it as full rotation
+//		raw_f[i] = theta_actual;
+
+		// storing the normalized angle every time the electrical angle 3PI/2 to calculate average zero electrical angle
+		if(i==(k*128+96))
+			{
+				_delay(50);
+				avg_elec_angle += _normalizeAngle(_wrapped.getMechanicalAngle()*_NPP);
+				k += 1;
+			}
+	}
+
+	_delay(2000);
+
+	/* 
+	backwards rotation
+	*/
+	Serial.println("Rotating backwards");
+	for(int i = 0; i<n_ticks; i++)
+	{                                                 
+		for(int j = 0; j<n2_ticks; j++)
+			{   
+				elec_angle -= deltaElectricalAngle;
+				motor.setPhaseVoltage(voltage_calibration, 0 ,elec_angle);
+			}
+
+			// delay to settle in position before taking a position sample
+			_delay(20);
+			_wrapped.update();
+			theta_actual = _wrapped.getAngle()-theta_init;
+			if (directionSensor == -1)
+			{
+			theta_actual = -theta_actual;
+			error_b[i] =  theta_actual - elec_angle/_NPP;
+			}
+			else
+			{
+			error_b[i] = elec_angle/_NPP - theta_actual;
+			}
+//			raw_b[i] = theta_actual;
+	}
+
+	// get post calibration mechanical angle.
+	_wrapped.update();
+	theta_absolute_post = _wrapped.getMechanicalAngle();
+
+	// done with the measurement
+	motor.setPhaseVoltage(0, 0, 0);
+
+	// raw offset from initial position in absolute radians between 0-2PI
+	float raw_offset = (theta_absolute_init+theta_absolute_post)/2;                   
+
+	// calculating the average zero electrica angle from the forward calibration.
+	motor.zero_electric_angle  = avg_elec_angle/_NPP;
+	Serial.print( "Average Zero Electrical Angle: ");
+	Serial.println( motor.zero_electric_angle); 
+
+	// Perform filtering to linearize position sensor eccentricity
+	// FIR n-sample average, where n = number of samples in one electrical cycle
+	// This filter has zero gain at electrical frequency and all integer multiples
+	// So cogging effects should be completely filtered out
+	float mean = 0;
+	for (int i = 0; i<n_ticks; i++){                                            //Average the forward and back directions
+		error[i] = 0.5f*(error_f[i] + error_b[n_ticks-i-1]);
+		}
+	for (int i = 0; i<n_ticks; i++){
+		for(int j = 0; j<window; j++){
+			int ind = -window/2 + j + i;                                    	// Indexes from -window/2 to + window/2
+			if(ind<0){
+				ind += n_ticks;}                                                // Moving average wraps around
+			else if(ind > n_ticks-1) {
+				ind -= n_ticks;}
+			error_filt[i] += error[ind]/(float)window;
+		}
+		mean += error_filt[i]/n_ticks;
+	}
+
+	// calculate offset index
+	int index_offset = floor(raw_offset/(_2PI/n_lut));
+
+	// Build Look Up Table
+	for (int i = 0; i<n_lut; i++){                                          
+		int ind =  index_offset + i*directionSensor;
+		if(ind > (n_lut-1)){ 
+			ind -= n_lut;
+		}
+		if(ind < 0 ){
+			ind += n_lut;
+		}
+		calibrationLut[ind] = (float) (error_filt[i*_NPP] - mean);
+		//Serial.print(ind);
+		//Serial.print('\t');
+		//Serial.println(calibrationLut[ind],5);
+		_delay(1);
+	}
+
+   // de-allocate memory
+    delete error_filt;
+    delete error;
+  //  delete raw_b;
+    delete error_b;
+  //  delete raw_f;
+    delete error_f;
+
+    Serial.println("Sensor Calibration Done.");
+
+}
+
+