try to fix submodule
This commit is contained in:
@@ -0,0 +1,44 @@
|
||||
/**
|
||||
* Encoder example code
|
||||
*
|
||||
* This is a code intended to test the encoder connections and to demonstrate the encoder setup.
|
||||
*
|
||||
*/
|
||||
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
|
||||
Encoder encoder = Encoder(2, 3, 8192);
|
||||
// interrupt routine intialisation
|
||||
void doA(){encoder.handleA();}
|
||||
void doB(){encoder.handleB();}
|
||||
|
||||
void setup() {
|
||||
// monitoring port
|
||||
Serial.begin(115200);
|
||||
|
||||
// enable/disable quadrature mode
|
||||
encoder.quadrature = Quadrature::ON;
|
||||
|
||||
// check if you need internal pullups
|
||||
encoder.pullup = Pullup::USE_EXTERN;
|
||||
|
||||
// initialise encoder hardware
|
||||
encoder.init();
|
||||
// hardware interrupt enable
|
||||
encoder.enableInterrupts(doA, doB);
|
||||
|
||||
Serial.println("Encoder ready");
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// iterative function updating the sensor internal variables
|
||||
// it is usually called in motor.loopFOC()
|
||||
// not doing much for the encoder though
|
||||
encoder.update();
|
||||
// display the angle and the angular velocity to the terminal
|
||||
Serial.print(encoder.getAngle());
|
||||
Serial.print("\t");
|
||||
Serial.println(encoder.getVelocity());
|
||||
}
|
||||
@@ -0,0 +1,57 @@
|
||||
/**
|
||||
* Encoder example code using only software interrupts
|
||||
*
|
||||
* This is a code intended to test the encoder connections and to
|
||||
* demonstrate the encoder setup fully using software interrupts.
|
||||
* - We use PciManager library: https://github.com/prampec/arduino-pcimanager
|
||||
*
|
||||
* This code will work on Arduino devices but not on STM32 devices
|
||||
*
|
||||
*/
|
||||
|
||||
#include <SimpleFOC.h>
|
||||
// software interrupt library
|
||||
#include <PciManager.h>
|
||||
#include <PciListenerImp.h>
|
||||
|
||||
// encoder instance
|
||||
Encoder encoder = Encoder(A0, A1, 2048);
|
||||
// interrupt routine intialisation
|
||||
void doA(){encoder.handleA();}
|
||||
void doB(){encoder.handleB();}
|
||||
|
||||
// encoder interrupt init
|
||||
PciListenerImp listenerA(encoder.pinA, doA);
|
||||
PciListenerImp listenerB(encoder.pinB, doB);
|
||||
|
||||
void setup() {
|
||||
// monitoring port
|
||||
Serial.begin(115200);
|
||||
|
||||
// enable/disable quadrature mode
|
||||
encoder.quadrature = Quadrature::ON;
|
||||
|
||||
// check if you need internal pullups
|
||||
encoder.pullup = Pullup::USE_EXTERN;
|
||||
|
||||
// initialise encoder hardware
|
||||
encoder.init();
|
||||
|
||||
// interrupt initialization
|
||||
PciManager.registerListener(&listenerA);
|
||||
PciManager.registerListener(&listenerB);
|
||||
|
||||
Serial.println("Encoder ready");
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// iterative function updating the sensor internal variables
|
||||
// it is usually called in motor.loopFOC()
|
||||
// not doing much for the encoder though
|
||||
encoder.update();
|
||||
// display the angle and the angular velocity to the terminal
|
||||
Serial.print(encoder.getAngle());
|
||||
Serial.print("\t");
|
||||
Serial.println(encoder.getVelocity());
|
||||
}
|
||||
@@ -0,0 +1,51 @@
|
||||
/**
|
||||
* Generic sensor example code
|
||||
*
|
||||
* This is a code intended to demonstrate how to implement the generic sensor class
|
||||
*
|
||||
*/
|
||||
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// sensor reading function example
|
||||
// for the magnetic sensor with analog communication
|
||||
// returning an angle in radians in between 0 and 2PI
|
||||
float readSensor(){
|
||||
return analogRead(A0)*_2PI/1024.0;
|
||||
}
|
||||
|
||||
// sensor intialising function
|
||||
void initSensor(){
|
||||
pinMode(A0,INPUT);
|
||||
}
|
||||
|
||||
// generic sensor class contructor
|
||||
// - read sensor callback
|
||||
// - init sensor callback (optional)
|
||||
GenericSensor sensor = GenericSensor(readSensor, initSensor);
|
||||
|
||||
void setup() {
|
||||
// monitoring port
|
||||
Serial.begin(115200);
|
||||
|
||||
// if callbacks are not provided in the constructor
|
||||
// they can be assigned directly:
|
||||
//sensor.readCallback = readSensor;
|
||||
//sensor.initCallback = initSensor;
|
||||
|
||||
sensor.init();
|
||||
|
||||
Serial.println("Sensor ready");
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// iterative function updating the sensor internal variables
|
||||
// it is usually called in motor.loopFOC()
|
||||
sensor.update();
|
||||
|
||||
// display the angle and the angular velocity to the terminal
|
||||
Serial.print(sensor.getAngle());
|
||||
Serial.print("\t");
|
||||
Serial.println(sensor.getVelocity());
|
||||
}
|
||||
@@ -0,0 +1,48 @@
|
||||
/**
|
||||
* Hall sensor example code
|
||||
*
|
||||
* This is a code intended to test the hall sensors connections and to demonstrate the hall sensor setup.
|
||||
*
|
||||
*/
|
||||
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// Hall sensor instance
|
||||
// HallSensor(int hallA, int hallB , int cpr, int index)
|
||||
// - hallA, hallB, hallC - HallSensor A, B and C pins
|
||||
// - pp - pole pairs
|
||||
HallSensor sensor = HallSensor(2, 3, 4, 14);
|
||||
|
||||
// Interrupt routine intialisation
|
||||
// channel A and B callbacks
|
||||
void doA(){sensor.handleA();}
|
||||
void doB(){sensor.handleB();}
|
||||
void doC(){sensor.handleC();}
|
||||
|
||||
|
||||
void setup() {
|
||||
// monitoring port
|
||||
Serial.begin(115200);
|
||||
|
||||
// check if you need internal pullups
|
||||
sensor.pullup = Pullup::USE_EXTERN;
|
||||
|
||||
// initialise encoder hardware
|
||||
sensor.init();
|
||||
// hardware interrupt enable
|
||||
sensor.enableInterrupts(doA, doB, doC);
|
||||
|
||||
Serial.println("Sensor ready");
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// iterative function updating the sensor internal variables
|
||||
// it is usually called in motor.loopFOC()
|
||||
sensor.update();
|
||||
// display the angle and the angular velocity to the terminal
|
||||
Serial.print(sensor.getAngle());
|
||||
Serial.print("\t");
|
||||
Serial.println(sensor.getVelocity());
|
||||
delay(100);
|
||||
}
|
||||
@@ -0,0 +1,59 @@
|
||||
/**
|
||||
* Hall sensors example code using only software interrupts
|
||||
*
|
||||
* This is a code intended to test the hall sensor connections and to
|
||||
* demonstrate the hall sensor setup fully using software interrupts.
|
||||
* - We use PciManager library: https://github.com/prampec/arduino-pcimanager
|
||||
*
|
||||
* This code will work on Arduino devices but not on STM32 devices
|
||||
*/
|
||||
|
||||
#include <SimpleFOC.h>
|
||||
// software interrupt library
|
||||
#include <PciManager.h>
|
||||
#include <PciListenerImp.h>
|
||||
|
||||
// Hall sensor instance
|
||||
// HallSensor(int hallA, int hallB , int cpr, int index)
|
||||
// - hallA, hallB, hallC - HallSensor A, B and C pins
|
||||
// - pp - pole pairs
|
||||
HallSensor sensor = HallSensor(2, 3, 4, 11);
|
||||
|
||||
// Interrupt routine intialisation
|
||||
// channel A and B callbacks
|
||||
void doA(){sensor.handleA();}
|
||||
void doB(){sensor.handleB();}
|
||||
void doC(){sensor.handleC();}
|
||||
// If no available hadware interrupt pins use the software interrupt
|
||||
PciListenerImp listenA(sensor.pinA, doA);
|
||||
PciListenerImp listenB(sensor.pinB, doB);
|
||||
PciListenerImp listenC(sensor.pinC, doC);
|
||||
|
||||
void setup() {
|
||||
// monitoring port
|
||||
Serial.begin(115200);
|
||||
|
||||
// check if you need internal pullups
|
||||
sensor.pullup = Pullup::USE_EXTERN;
|
||||
|
||||
// initialise encoder hardware
|
||||
sensor.init();
|
||||
|
||||
// software interrupts
|
||||
PciManager.registerListener(&listenA);
|
||||
PciManager.registerListener(&listenB);
|
||||
PciManager.registerListener(&listenC);
|
||||
|
||||
Serial.println("Sensor ready");
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// iterative function updating the sensor internal variables
|
||||
// it is usually called in motor.loopFOC()
|
||||
sensor.update();
|
||||
// display the angle and the angular velocity to the terminal
|
||||
Serial.print(sensor.getAngle());
|
||||
Serial.print("\t");
|
||||
Serial.println(sensor.getVelocity());
|
||||
}
|
||||
@@ -0,0 +1,62 @@
|
||||
/**
|
||||
* An example to find the center offsets for both ADC channels used in the LinearHall sensor constructor
|
||||
* Spin your motor through at least one full revolution to average out all of the variations in magnet strength.
|
||||
*/
|
||||
|
||||
//Change these defines to match the analog input pins that your hall sensors are connected to
|
||||
#define LINEAR_HALL_CHANNEL_A 39
|
||||
#define LINEAR_HALL_CHANNEL_B 33
|
||||
|
||||
|
||||
//program variables
|
||||
int minA, maxA, minB, maxB, centerA, centerB;
|
||||
unsigned long timestamp;
|
||||
|
||||
void setup() {
|
||||
// monitoring port
|
||||
Serial.begin(115200);
|
||||
|
||||
// initialise magnetic sensor hardware
|
||||
pinMode(LINEAR_HALL_CHANNEL_A, INPUT);
|
||||
pinMode(LINEAR_HALL_CHANNEL_B, INPUT);
|
||||
|
||||
minA = analogRead(LINEAR_HALL_CHANNEL_A);
|
||||
maxA = minA;
|
||||
centerA = (minA + maxA) / 2;
|
||||
minB = analogRead(LINEAR_HALL_CHANNEL_B);
|
||||
maxB = minB;
|
||||
centerB = (minB + maxB) / 2;
|
||||
|
||||
Serial.println("Sensor ready");
|
||||
delay(1000);
|
||||
timestamp = millis();
|
||||
}
|
||||
|
||||
void loop() {
|
||||
//read sensors and update variables
|
||||
int tempA = analogRead(LINEAR_HALL_CHANNEL_A);
|
||||
if (tempA < minA) minA = tempA;
|
||||
if (tempA > maxA) maxA = tempA;
|
||||
centerA = (minA + maxA) / 2;
|
||||
int tempB = analogRead(LINEAR_HALL_CHANNEL_B);
|
||||
if (tempB < minB) minB = tempB;
|
||||
if (tempB > maxB) maxB = tempB;
|
||||
centerB = (minB + maxB) / 2;
|
||||
|
||||
if (millis() > timestamp + 100) {
|
||||
timestamp = millis();
|
||||
// display the center counts, and max and min count
|
||||
Serial.print("A:");
|
||||
Serial.print(centerA);
|
||||
Serial.print("\t, B:");
|
||||
Serial.print(centerB);
|
||||
Serial.print("\t, min A:");
|
||||
Serial.print(minA);
|
||||
Serial.print("\t, max A:");
|
||||
Serial.print(maxA);
|
||||
Serial.print("\t, min B:");
|
||||
Serial.print(minB);
|
||||
Serial.print("\t, max B:");
|
||||
Serial.println(maxB);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,56 @@
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
/**
|
||||
* An example to find out the raw max and min count to be provided to the constructor
|
||||
* Spin your motor/sensor/magnet to see what is the maximum output of the sensor and what is the minimum value
|
||||
* And replace values 14 and 1020 with new values. Once when you replace them make sure there is no jump in the angle reading sensor.getAngle().
|
||||
* If there is a jump that means you can still find better values.
|
||||
*/
|
||||
|
||||
/**
|
||||
* Magnetic sensor reading analog voltage on pin A1. This voltage is proportional to rotation position.
|
||||
* Tested on AS5600 magnetic sensor running in 'analog mode'. Note AS5600 works better in 'i2C mode' (less noise) but only supports one sensor per i2c bus.
|
||||
*
|
||||
* MagneticSensorAnalog(uint8_t _pinAnalog, int _min, int _max)
|
||||
* - pinAnalog - the pin that is reading the pwm from magnetic sensor
|
||||
* - min_raw_count - the smallest expected reading. Whilst you might expect it to be 0 it is often ~15. Getting this wrong results in a small click once per revolution
|
||||
* - max_raw_count - the largest value read. whilst you might expect it to be 2^10 = 1023 it is often ~ 1020. Note ESP32 will be closer to 4096 with its 12bit ADC
|
||||
*/
|
||||
MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020);
|
||||
|
||||
void setup() {
|
||||
// monitoring port
|
||||
Serial.begin(115200);
|
||||
|
||||
// initialise magnetic sensor hardware
|
||||
sensor.init();
|
||||
|
||||
Serial.println("Sensor ready");
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
int max_count = 0;
|
||||
int min_count = 100000;
|
||||
|
||||
void loop() {
|
||||
// iterative function updating the sensor internal variables
|
||||
// it is usually called in motor.loopFOC()
|
||||
// this function reads the sensor hardware and
|
||||
// has to be called before getAngle nad getVelocity
|
||||
sensor.update();
|
||||
|
||||
// keep track of min and max
|
||||
if(sensor.raw_count > max_count) max_count = sensor.raw_count;
|
||||
else if(sensor.raw_count < min_count) min_count = sensor.raw_count;
|
||||
|
||||
// display the raw count, and max and min raw count
|
||||
Serial.print("angle:");
|
||||
Serial.print(sensor.getAngle());
|
||||
Serial.print("\t, raw:");
|
||||
Serial.print(sensor.raw_count);
|
||||
Serial.print("\t, min:");
|
||||
Serial.print(min_count);
|
||||
Serial.print("\t, max:");
|
||||
Serial.println(max_count);
|
||||
delay(100);
|
||||
}
|
||||
@@ -0,0 +1,37 @@
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* Magnetic sensor reading analog voltage on pin A1. This voltage is proportional to rotation position.
|
||||
* Tested on AS5600 magnetic sensor running in 'analog mode'. Note AS5600 works better in 'i2C mode' (less noise) but only supports one sensor per i2c bus.
|
||||
*
|
||||
* MagneticSensorAnalog(uint8_t _pinAnalog, int _min, int _max)
|
||||
* - pinAnalog - the pin that is reading the pwm from magnetic sensor
|
||||
* - min_raw_count - the smallest expected reading. Whilst you might expect it to be 0 it is often ~15. Getting this wrong results in a small click once per revolution
|
||||
* - max_raw_count - the largest value read. whilst you might expect it to be 2^10 = 1023 it is often ~ 1020. Note ESP32 will be closer to 4096 with its 12bit ADC
|
||||
*/
|
||||
MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020);
|
||||
|
||||
void setup() {
|
||||
// monitoring port
|
||||
Serial.begin(115200);
|
||||
|
||||
// initialise magnetic sensor hardware
|
||||
sensor.init();
|
||||
|
||||
Serial.println("Sensor ready");
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// iterative function updating the sensor internal variables
|
||||
// it is usually called in motor.loopFOC()
|
||||
// this function reads the sensor hardware and
|
||||
// has to be called before getAngle nad getVelocity
|
||||
sensor.update();
|
||||
// display the angle and the angular velocity to the terminal
|
||||
Serial.print(sensor.getAngle());
|
||||
Serial.print("\t");
|
||||
Serial.println(sensor.getVelocity());
|
||||
}
|
||||
@@ -0,0 +1,40 @@
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
/** Annoyingly some i2c sensors (e.g. AS5600) have a fixed chip address. This means only one of these devices can be addressed on a single bus
|
||||
* This example shows how a second i2c bus can be used to communicate with a second sensor.
|
||||
*/
|
||||
|
||||
MagneticSensorI2C sensor0 = MagneticSensorI2C(AS5600_I2C);
|
||||
MagneticSensorI2C sensor1 = MagneticSensorI2C(AS5600_I2C);
|
||||
|
||||
|
||||
void setup() {
|
||||
|
||||
Serial.begin(115200);
|
||||
_delay(750);
|
||||
|
||||
Wire.setClock(400000);
|
||||
Wire1.setClock(400000);
|
||||
|
||||
// Normally SimpleFOC will call begin for i2c but with esp32 begin() is the only way to set pins!
|
||||
// It seems safe to call begin multiple times
|
||||
Wire1.begin(19, 23, (uint32_t)400000);
|
||||
|
||||
sensor0.init();
|
||||
sensor1.init(&Wire1);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// iterative function updating the sensor internal variables
|
||||
// it is usually called in motor.loopFOC()
|
||||
// this function reads the sensor hardware and
|
||||
// has to be called before getAngle nad getVelocity
|
||||
sensor0.update();
|
||||
sensor1.update();
|
||||
|
||||
_delay(200);
|
||||
Serial.print(sensor0.getAngle());
|
||||
Serial.print(" - ");
|
||||
Serial.print(sensor1.getAngle());
|
||||
Serial.println();
|
||||
}
|
||||
@@ -0,0 +1,39 @@
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
/** Annoyingly some i2c sensors (e.g. AS5600) have a fixed chip address. This means only one of these devices can be addressed on a single bus
|
||||
* This example shows how a second i2c bus can be used to communicate with a second sensor.
|
||||
*/
|
||||
|
||||
MagneticSensorI2C sensor0 = MagneticSensorI2C(AS5600_I2C);
|
||||
MagneticSensorI2C sensor1 = MagneticSensorI2C(AS5600_I2C);
|
||||
|
||||
// example of stm32 defining 2nd bus
|
||||
TwoWire Wire1(PB11, PB10);
|
||||
|
||||
|
||||
void setup() {
|
||||
|
||||
Serial.begin(115200);
|
||||
_delay(750);
|
||||
|
||||
Wire.setClock(400000);
|
||||
Wire1.setClock(400000);
|
||||
|
||||
sensor0.init();
|
||||
sensor1.init(&Wire1);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// iterative function updating the sensor internal variables
|
||||
// it is usually called in motor.loopFOC()
|
||||
// this function reads the sensor hardware and
|
||||
// has to be called before getAngle nad getVelocity
|
||||
sensor0.update();
|
||||
sensor1.update();
|
||||
|
||||
_delay(200);
|
||||
Serial.print(sensor0.getAngle());
|
||||
Serial.print(" - ");
|
||||
Serial.print(sensor1.getAngle());
|
||||
Serial.println();
|
||||
}
|
||||
@@ -0,0 +1,43 @@
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// MagneticSensorI2C(uint8_t _chip_address, float _cpr, uint8_t _angle_register_msb)
|
||||
// chip_address I2C chip address
|
||||
// bit_resolution resolution of the sensor
|
||||
// angle_register_msb angle read register msb
|
||||
// bits_used_msb number of used bits in msb register
|
||||
//
|
||||
// make sure to read the chip address and the chip angle register msb value from the datasheet
|
||||
// also in most cases you will need external pull-ups on SDA and SCL lines!!!!!
|
||||
//
|
||||
// For AS5058B
|
||||
// MagneticSensorI2C sensor = MagneticSensorI2C(0x40, 14, 0xFE, 8);
|
||||
|
||||
// Example of AS5600 configuration
|
||||
MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
|
||||
|
||||
|
||||
void setup() {
|
||||
// monitoring port
|
||||
Serial.begin(115200);
|
||||
|
||||
// configure i2C
|
||||
Wire.setClock(400000);
|
||||
// initialise magnetic sensor hardware
|
||||
sensor.init();
|
||||
|
||||
Serial.println("Sensor ready");
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// iterative function updating the sensor internal variables
|
||||
// it is usually called in motor.loopFOC()
|
||||
// this function reads the sensor hardware and
|
||||
// has to be called before getAngle nad getVelocity
|
||||
sensor.update();
|
||||
|
||||
// display the angle and the angular velocity to the terminal
|
||||
Serial.print(sensor.getAngle());
|
||||
Serial.print("\t");
|
||||
Serial.println(sensor.getVelocity());
|
||||
}
|
||||
@@ -0,0 +1,49 @@
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
|
||||
/**
|
||||
* An example to find out the raw max and min count to be provided to the constructor
|
||||
* SPin your motor/sensor/magnet to see what is the maximum output of the sensor and what is the minimum value
|
||||
* And replace values 4 and 904 with new values. Once when you replace them make sure there is no jump in the angle reading sensor.getAngle().
|
||||
* If there is a jump that means you can still find better values.
|
||||
*/
|
||||
MagneticSensorPWM sensor = MagneticSensorPWM(2, 4, 904);
|
||||
void doPWM(){sensor.handlePWM();}
|
||||
|
||||
void setup() {
|
||||
// monitoring port
|
||||
Serial.begin(115200);
|
||||
|
||||
// initialise magnetic sensor hardware
|
||||
sensor.init();
|
||||
// comment out to use sensor in blocking (non-interrupt) way
|
||||
sensor.enableInterrupt(doPWM);
|
||||
|
||||
Serial.println("Sensor ready");
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
int max_pulse= 0;
|
||||
int min_pulse = 10000;
|
||||
|
||||
void loop() {
|
||||
// iterative function updating the sensor internal variables
|
||||
// it is usually called in motor.loopFOC()
|
||||
// this function reads the sensor hardware and
|
||||
// has to be called before getAngle nad getVelocity
|
||||
sensor.update();
|
||||
|
||||
// keep track of min and max
|
||||
if(sensor.pulse_length_us > max_pulse) max_pulse = sensor.pulse_length_us;
|
||||
else if(sensor.pulse_length_us < min_pulse) min_pulse = sensor.pulse_length_us;
|
||||
|
||||
// display the raw count, and max and min raw count
|
||||
Serial.print("angle:");
|
||||
Serial.print(sensor.getAngle());
|
||||
Serial.print("\t, raw:");
|
||||
Serial.print(sensor.pulse_length_us);
|
||||
Serial.print("\t, min:");
|
||||
Serial.print(min_pulse);
|
||||
Serial.print("\t, max:");
|
||||
Serial.println(max_pulse);
|
||||
}
|
||||
@@ -0,0 +1,38 @@
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
|
||||
/**
|
||||
* Magnetic sensor reading pwm signal on pin 2. The pwm duty cycle is proportional to the sensor angle.
|
||||
*
|
||||
* MagneticSensorPWM(uint8_t MagneticSensorPWM, int _min, int _max)
|
||||
* - pinPWM - the pin that is reading the pwm from magnetic sensor
|
||||
* - min_raw_count - the smallest expected reading. Whilst you might expect it to be 0 it is often ~5. Getting this wrong results in a small click once per revolution
|
||||
* - max_raw_count - the largest value read. whilst you might expect it to be 1kHz = 1000 it is often ~910. depending on the exact frequency and saturation
|
||||
*/
|
||||
MagneticSensorPWM sensor = MagneticSensorPWM(2, 4, 904);
|
||||
void doPWM(){sensor.handlePWM();}
|
||||
|
||||
void setup() {
|
||||
// monitoring port
|
||||
Serial.begin(115200);
|
||||
|
||||
// initialise magnetic sensor hardware
|
||||
sensor.init();
|
||||
// comment out to use sensor in blocking (non-interrupt) way
|
||||
sensor.enableInterrupt(doPWM);
|
||||
|
||||
Serial.println("Sensor ready");
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// iterative function updating the sensor internal variables
|
||||
// it is usually called in motor.loopFOC()
|
||||
// this function reads the sensor hardware and
|
||||
// has to be called before getAngle nad getVelocity
|
||||
sensor.update();
|
||||
// display the angle and the angular velocity to the terminal
|
||||
Serial.print(sensor.getAngle());
|
||||
Serial.print("\t");
|
||||
Serial.println(sensor.getVelocity());
|
||||
}
|
||||
@@ -0,0 +1,44 @@
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// software interrupt library
|
||||
#include <PciManager.h>
|
||||
#include <PciListenerImp.h>
|
||||
|
||||
/**
|
||||
* Magnetic sensor reading analog voltage on pin which does not have hardware interrupt support. Such as A0.
|
||||
*
|
||||
* MagneticSensorPWM(uint8_t MagneticSensorPWM, int _min, int _max)
|
||||
* - pinPWM - the pin that is reading the pwm from magnetic sensor
|
||||
* - min_raw_count - the smallest expected reading. Whilst you might expect it to be 0 it is often ~5. Getting this wrong results in a small click once per revolution
|
||||
* - max_raw_count - the largest value read. whilst you might expect it to be 1kHz = 1000 it is often ~910. depending on the exact frequency and saturation
|
||||
*/
|
||||
MagneticSensorPWM sensor = MagneticSensorPWM(A0, 4, 904);
|
||||
void doPWM(){sensor.handlePWM();}
|
||||
|
||||
// encoder interrupt init
|
||||
PciListenerImp listenerPWM(sensor.pinPWM, doPWM);
|
||||
|
||||
void setup() {
|
||||
// monitoring port
|
||||
Serial.begin(115200);
|
||||
|
||||
// initialise magnetic sensor hardware
|
||||
sensor.init();
|
||||
// comment out to use sensor in blocking (non-interrupt) way
|
||||
PciManager.registerListener(&listenerPWM);
|
||||
|
||||
Serial.println("Sensor ready");
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// iterative function updating the sensor internal variables
|
||||
// it is usually called in motor.loopFOC()
|
||||
// this function reads the sensor hardware and
|
||||
// has to be called before getAngle nad getVelocity
|
||||
sensor.update();
|
||||
// display the angle and the angular velocity to the terminal
|
||||
Serial.print(sensor.getAngle());
|
||||
Serial.print("\t");
|
||||
Serial.println(sensor.getVelocity());
|
||||
}
|
||||
@@ -0,0 +1,41 @@
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// alternative pinout
|
||||
#define HSPI_MISO 12
|
||||
#define HSPI_MOSI 13
|
||||
#define HSPI_SCLK 14
|
||||
#define HSPI_SS 15
|
||||
|
||||
// MagneticSensorSPI(int cs, float _cpr, int _angle_register)
|
||||
// config - SPI config
|
||||
// cs - SPI chip select pin
|
||||
MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, HSPI_SS);
|
||||
|
||||
// for esp 32, it has 2 spi interfaces VSPI (default) and HPSI as the second one
|
||||
// to enable it instatiate the object
|
||||
SPIClass SPI_2(HSPI);
|
||||
|
||||
void setup() {
|
||||
// monitoring port
|
||||
Serial.begin(115200);
|
||||
|
||||
// start the newly defined spi communication
|
||||
SPI_2.begin(HSPI_SCLK, HSPI_MISO, HSPI_MOSI, HSPI_SS); //SCLK, MISO, MOSI, SS
|
||||
// initialise magnetic sensor hardware
|
||||
sensor.init(&SPI_2);
|
||||
|
||||
Serial.println("Sensor ready");
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// iterative function updating the sensor internal variables
|
||||
// it is usually called in motor.loopFOC()
|
||||
// this function reads the sensor hardware and
|
||||
// has to be called before getAngle nad getVelocity
|
||||
sensor.update();
|
||||
// display the angle and the angular velocity to the terminal
|
||||
Serial.print(sensor.getAngle());
|
||||
Serial.print("\t");
|
||||
Serial.println(sensor.getVelocity());
|
||||
}
|
||||
@@ -0,0 +1,32 @@
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// MagneticSensorSPI(int cs, float _cpr, int _angle_register)
|
||||
// config - SPI config
|
||||
// cs - SPI chip select pin
|
||||
MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, PA15);
|
||||
|
||||
// these are valid pins (mosi, miso, sclk) for 2nd SPI bus on storm32 board (stm32f107rc)
|
||||
SPIClass SPI_2(PB15, PB14, PB13);
|
||||
|
||||
void setup() {
|
||||
// monitoring port
|
||||
Serial.begin(115200);
|
||||
|
||||
// initialise magnetic sensor hardware
|
||||
sensor.init(&SPI_2);
|
||||
|
||||
Serial.println("Sensor ready");
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// iterative function updating the sensor internal variables
|
||||
// it is usually called in motor.loopFOC()
|
||||
// this function reads the sensor hardware and
|
||||
// has to be called before getAngle nad getVelocity
|
||||
sensor.update();
|
||||
// display the angle and the angular velocity to the terminal
|
||||
Serial.print(sensor.getAngle());
|
||||
Serial.print("\t");
|
||||
Serial.println(sensor.getVelocity());
|
||||
}
|
||||
@@ -0,0 +1,32 @@
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// MagneticSensorSPI(MagneticSensorSPIConfig_s config, int cs)
|
||||
// config - SPI config
|
||||
// cs - SPI chip select pin
|
||||
// magnetic sensor instance - SPI
|
||||
MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, 10);
|
||||
// alternative constructor (chipselsect, bit_resolution, angle_read_register, )
|
||||
// MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF);
|
||||
|
||||
void setup() {
|
||||
// monitoring port
|
||||
Serial.begin(115200);
|
||||
|
||||
// initialise magnetic sensor hardware
|
||||
sensor.init();
|
||||
|
||||
Serial.println("Sensor ready");
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// iterative function updating the sensor internal variables
|
||||
// it is usually called in motor.loopFOC()
|
||||
// this function reads the sensor hardware and
|
||||
// has to be called before getAngle nad getVelocity
|
||||
sensor.update();
|
||||
// display the angle and the angular velocity to the terminal
|
||||
Serial.print(sensor.getAngle());
|
||||
Serial.print("\t");
|
||||
Serial.println(sensor.getVelocity());
|
||||
}
|
||||
Reference in New Issue
Block a user