Again, thank you very much for your help and your clear explanation especially.
Based on ‘share and learn’ I learned something again and like to share my fully working code below. Finally my Force-Feed-Back-Flight-Yoke-Project could start.
So, here’s my initial code:
…
/* Example for Gamecontroller, intended to use for flight simulator (preparation for force feed back).
-
+++Tested and used with the following hardware+++:
-
DEVICE 1: Microcontroller
-
TEENSY 3.2 (3,3V, but 5V tolerant on inputs)
-
special USB feature: emulates HID Joystick / game controller
-
DEVICE 2: Level Shifter
-
connect Teensy 3.2 with SimpleFOCShield V2.0.3 via levelshifter,
-
(also tested without levelshifter, seems to work also…)
-
DEVICE 3: BLDC Shield
-
SimpleFOCShield V2.0.3
-
DEVICE 4: Gimbal Motor
-
iPower GM5208-24, 25.6 ohm phase resistance
-
DEVICE 5: Angle sensor
-
AS5048A, magnetic sensor, details: iPower Motor and SPI Wiring
-
pin connection:
-
-
| Dev 1 | DEV 2 | DEV 3 | DEV 4 | DEV 5 |
-
| Teensy | Levelshifter | SimpleFOCShield | Motor | Sensor A5048A |
-
|---------|----------------|-----------------|---------|----------------------------|
-
| | | A | phase A | |
-
| | | B | phase B | |
-
| | | C | phase C | |
-
| Vin | HV | 5V | | VDD5V, pin 11, green wire |
-
| 3V3 | LV | | | |
-
| 5 | LV1 HV1 | 5 | | |
-
| 6 | LV2 HV2 | 6 | | |
-
| 8 | LV3 HV3 | 8 | | |
-
| 9 | LV4 HV4 | 9 | | |
-
| 10 | | | | CS, pin 1, white wire |
-
| 11 | | | | MOSI, pin 4, black wire |
-
| 12 | | | | MISO, pin 3, yellow wire |
-
| 13 | | | | CLK, pin 2, red wire |
-
| GND | GND GND | GND | | GND, pin 13, purple wire |
-
*/
// included libraries
#include <Arduino.h>
#include <SimpleFOC.h>
#define DEBUGmode // use only for test purpose, reduces performance!
//#define UseCommander // use only for test purpose, reduces performance!
#define MotorConnected
#define Joystickmodus
// MagneticSensorSPI(MagneticSensorSPIConfig_s config, int cs)
// config - SPI config
// cs - SPI chip select pin
// magnetic sensor instance - SPI
MagneticSensorSPI sensor1 = MagneticSensorSPI(AS5147_SPI, 10); // AS50548A in this example, controlled ‘equal’ to AS5147!
MagneticSensorI2C sensor2 = MagneticSensorI2C(0x36, 12, 0x0E, 4); // AS5600 in this example
// alternative constructor (chipselect, bit_resolution, angle_read_register, )
// MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF);
#ifdef DEBUGmode
unsigned long previousMillis = 0; // obvious
const long interval = 1000; // every 1000ms
const int HIDbuttons = 32; // Teensy as joystick offers 32 buttons
int buttoncounter = 1; // counter for button number
bool btnstate = false; //
int hatangle = 0; // valid values: 0, 45, 90, 135, 180, 225, 270, 315
#endif
#ifdef MotorConnected
// BLDCMotor motor = BLDCMotor(number of polpairs); // typically 11 with gimbal motor
BLDCMotor motor1 = BLDCMotor(11, 25.6); // iPower GM5208-24 has 25.6 ohm phase resistance
//BLDCMotor motor2 = BLDCMotor(11, 11.3); // in preparation, for next stage…
// BLDCDriver3PWM driver = BLDCDriver3PWM(IN_A, IN_B, IN_C, EN);
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);
// set initial point variable
float var_target = 0.5; // initial value, don’t set to high, otherwise initial oscillation of motor might occur!
#ifdef UseCommander
// initiate the commander
Commander command = Commander(Serial);
void doTarget(char *cmd) { command.scalar(&var_target, cmd); }
#endif
#endif
#ifdef Joystickmodus
float twoPI = 2.0 * 3.14159; // full circle in [rad]
float joy_A_center = -0.632; // Joystick Axis A, depending on physical fixed magnet on shaft orientation
float joy_A_MIN = joy_A_center - 0.9; // Joystick Axis A, lower border limit
float joy_A_MAX = joy_A_center + 0.9; // Joystick Axis A, upper border limit
float joy_A_ABS; // Joystick Axis A, absolute angle in [rad], forces initial calculation
float joy_B_center = -0.512; // Joystick Axis B, depending on physical fixed magnet on shaft orientation
float joy_B_MIN = joy_B_center - 0.1; // Joystick Axis B, lower border limit
float joy_B_MAX = joy_B_center + 0.1; // Joystick Axis B, upper border limit
float joy_B_ABS; // Joystick Axis B, absolute angle in [rad], forces initial calculation
/*******************
*******************/
void fetch_absANGLE()
{
// 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 and getVelocity
#ifndef MotorConnected
sensor1.update();
#endif
// get current sensor Angle, may vary from 0…2*Pi
float curr = sensor1.getSensorAngle();
// get full rotations, positive or negative
int fc = sensor1.getFullRotations();
// calculate absolute value
if (fc >= 0)
{
// absolute value if positive fullrotations:
joy_A_ABS = curr + (fc * twoPI);
}
else
{
// absolute value if negative fullrotations:
joy_A_ABS = -(twoPI - curr) + ((fc + 1) * twoPI);
}
}
#endif
/*******************
*******************/
void setup()
{
#ifdef DEBUGmode
Serial.begin(115200);
delay(500);
#endif
// initialize sensor one
sensor1.init();
// initialize sensor two
sensor2.init();
#ifdef MotorConnected
// link the motor to the sensor
motor1.linkSensor(&sensor1);
// power supply voltage, set nominal value from power supply
driver.voltage_power_supply = 24;
// OPTIONAL: Max DC voltage allowed
// if set to low oscillation might occur, if set to high overheating of motor and driver might occur
driver.voltage_limit = 24.0; // optional
// set driver PWM frequency, OPTIONAL: BLDCDriver 3PWM | Arduino-FOC
// default pwm frequency in SimpleFOC library for TEENSY 3.2 should be 50kHz.
// Mesuring with oscilloscope results in only 25kHz default.
// In any case: don’t forget to add ‘lib_archive = false’ to platformio.ini,
// otherwise the normal pwm frequency of 490Hz will be used!
// driver.pwm_frequency = 50000; // optional
// finally driver initialization
driver.init();
// link driver with motor
motor1.linkDriver(&driver);
// set voltage for sensor align, must be at least to move motor during sensor ini
motor1.voltage_sensor_align = 5.0;
// choose FOC modulation (optional)
motor1.foc_modulation = FOCModulationType::SpaceVectorPWM;
// set motion control loop to be used
motor1.controller = MotionControlType::torque;
// optional, not tested yet:
// motor1.PID_velocity.P = 1;
// motor1.PID_velocity.I = 1;
// motor1.PID_velocity.D = 1;
// initialize motor
motor1.init();
// align sensor and start FOC
motor1.initFOC();
#ifdef UseCommander
// add target command T
command.add(‘T’, doTarget, “target voltage”);
#endif
#endif
Serial.println(“Sensor ready”);
_delay(1000);
#ifdef Joystickmodus
fetch_absANGLE();
#endif
}
/*******************
*******************/
void loop()
{
#ifdef MotorConnected
// main FOC algorithm function
motor1.loopFOC();
// reads current angle
float w = sensor1.getAngle();
sensor2.update(); // neccessary if not loopFOC running for this motor/sensor !?
float z = sensor2.getAngle();
// Motion control function
// velocity, position or voltage (defined in motor.controller)
// this function can be run at much lower frequency than loopFOC() function
// You can also use motor.move() and set the motor.target in the code
//motor.move(w * var_target); // IF Gimbal motor rotates in other direcdtion
motor1.move(-(joy_A_center - w) * var_target);
//motor.move(w * var_target);
#ifdef UseCommander
// user communication
command.run();
#endif
#endif
#ifdef Joystickmodus
fetch_absANGLE();
if ((joy_A_ABS >= joy_A_MIN) && (joy_A_ABS <= joy_A_MAX)){
int jsv = map(joy_A_ABS, joy_A_MIN, joy_A_MAX, 0, 1023); // joystick values typically vary from 0…1023
Joystick.X(jsv);
Joystick.Y(jsv);
Joystick.Z(jsv);
Joystick.Zrotate(jsv);
Joystick.sliderLeft(jsv);
Joystick.sliderRight(jsv);
}
#endif
#ifdef DEBUGmode
unsigned long currentMillis = millis();
if (currentMillis - previousMillis >= interval)
{
btnstate = !btnstate; // toggles state
Joystick.button(buttoncounter, btnstate);
if (btnstate == false){
if (buttoncounter < HIDbuttons){
buttoncounter++;}
else{buttoncounter =1;}
}
Serial.print("Absolute angle [rad]:");
Serial.print(joy_A_ABS);
Serial.print("\t\t");
Serial.print("joy_A_MIN:");
Serial.print(joy_A_MIN);
Serial.print("\t\t");
Serial.print("joy_A_center");
Serial.print(joy_A_center);
Serial.print("\t\t");
Serial.print("joy_A_MAX:");
Serial.println(joy_A_MAX);
Serial.print("Sensor2:");
Serial.println(z);
Joystick.hat(hatangle);
if (hatangle == 360){
hatangle =0;
}else{hatangle = hatangle +45;}
previousMillis = currentMillis;
}
#endif
}