Hi, I am using an arduino due and two simple foc shields. I want to controll the angle of each motor with a Poti. But my 2nd motor just does whatever the first one does but without controller. I think one shield is some how controlling both motors. Can some one Help?
My code
#include <Arduino.h>
#include <SimpleFOC.h>
#include <seesaw_control.h>
#include <stdio.h>
#include <stdint.h>
const double zweiPi = 2 * 3.14159265359;
const double maxV = 12;
const double maxA = 1.5;
int Poti_1 = 0; // Potentiometer 1
int Poti_2 = 0; // Potentiometer 2
#define PotiPin_1 A7 // Analog-Pin, an dem das Poti hängt
#define PotiPin_2 A8 // Analog-Pin, an dem das Poti hängt
const int PotiMaxWert = 1023; // 10 Bit ADC (0-1023)
//MOTOR u sensor 1
BLDCMotor motor_1 = BLDCMotor(11);
BLDCDriver3PWM driver_1 = BLDCDriver3PWM(9, 5, 6, 8);
MagneticSensorSPI sensor_1 = MagneticSensorSPI(2, 14, 0x3FFF);
InlineCurrentSense current_sense_1 = InlineCurrentSense(0.005f, 50.0f, A0, A2);
BLDCMotor motor_2 = BLDCMotor(11);
BLDCDriver3PWM driver_2 = BLDCDriver3PWM(10, 12, 13, 11);
MagneticSensorSPI sensor_2 = MagneticSensorSPI(7, 14, 0x3FFF);
InlineCurrentSense current_sense_2 = InlineCurrentSense(0.01f, 50.0f, A1, A3);
// Commander Interface Instance
Commander command = Commander(Serial);
// Custom Monitor Function
void customMonitor(BLDCMotor& motor, int aaa) {
static int monitor_counter = 0;
if (++monitor_counter > 2000) {
monitor_counter = 0;
Serial.print("Motor: ");
Serial.print(aaa);
Serial.print("Target: ");
Serial.print(motor.target, 2);
Serial.print("\tAngle: ");
Serial.print(motor.shaft_angle, 2);
Serial.print("\tVelocity: ");
Serial.println(motor.shaft_velocity, 2);
}
}
void configureMotor(BLDCMotor& motor) {
motor.foc_modulation = FOCModulationType::SinePWM;
motor.torque_controller = TorqueControlType::voltage;
motor.controller = MotionControlType::angle;
motor.motion_downsample = 0;
// Velocity PID-Controller Parameters
motor.PID_velocity.P = 0.5;
motor.PID_velocity.I = 10.0;
motor.PID_velocity.D = 0.0;
motor.PID_velocity.output_ramp = 1000.0;
motor.PID_velocity.limit = 20.0;
motor.LPF_velocity.Tf = 0.02;
// Angle PID-Controller Parameters
motor.P_angle.P = 15.0;
motor.P_angle.I = 1.0;
motor.P_angle.D = 0.0;
motor.P_angle.output_ramp = 0.0;
motor.P_angle.limit = 20.0;
motor.LPF_angle.Tf = 0.0;
// Current q PID-Controller Parameters
motor.PID_current_q.P = 3.0;
motor.PID_current_q.I = 300.0;
motor.PID_current_q.D = 0.0;
motor.PID_current_q.output_ramp = 0.0;
motor.PID_current_q.limit = 12.0;
motor.LPF_current_q.Tf = 0.005;
// Current d PID-Controller Parameters
motor.PID_current_d.P = 3.0;
motor.PID_current_d.I = 300.0;
motor.PID_current_d.D = 0.0;
motor.PID_current_d.output_ramp = 0.0;
motor.PID_current_d.limit = 12.0;
motor.LPF_current_d.Tf = 0.005;
// Motor Limits
motor.velocity_limit = 18.0;
motor.voltage_limit = 6;
motor.current_limit = 2;
}
void configureMotor(BLDCDriver3PWM& driver) {
driver_1.pwm_frequency = 20000;
driver_1.voltage_power_supply = 24;
}
void doMotor(char* cmd){ command.motor(&motor_1, cmd); }
void setup() {
Serial.begin(115200);
analogReadResolution(10); // 10-Bit Auflösung (0-1023)
// Initialize Sensor
sensor_1.init();
motor_1.linkSensor(&sensor_1);
driver_1.init();
motor_1.linkDriver(&driver_1);
current_sense_1.linkDriver(&driver_1);
configureMotor(motor_1); // Configure Motor Parameters
motor_1.useMonitoring(Serial); // Aktivieren der Motorüberwachung
motor_1.monitor_downsample = 1000;
motor_1.init();
current_sense_1.init();
current_sense_1.skip_align = true;
motor_1.linkCurrentSense(¤t_sense_1);
motor_1.initFOC();
_delay(50);
sensor_2.init();
motor_2.linkSensor(&sensor_2);
driver_2.init();
motor_2.linkDriver(&driver_2);
current_sense_2.linkDriver(&driver_2);
configureMotor(motor_2); // Configure Motor Parameters
motor_2.useMonitoring(Serial); // Aktivieren der Motorüberwachung
motor_2.monitor_downsample = 1000; // Downsample the monitoring to every 1000th loop
motor_2.init();
current_sense_2.init();
current_sense_2.skip_align = true;
motor_2.linkCurrentSense(¤t_sense_2);
motor_2.initFOC();
_delay(50);
// Set Initial Target
motor_1.target = 0.0;
motor_2.target = 0.0;
// Subscribe Motor Commander
command.add(‘M’, doMotor, “motor”);
_delay(50);
}
void loop() {
float potiWert_1 = analogRead(PotiPin_1); // Potentiometer auslesen
float winkel_poti_1 = (potiWert_1 / PotiMaxWert)* zweiPi;
float potiWert_2 = analogRead(PotiPin_2); // Potentiometer auslesen
float winkel_poti_2 = (potiWert_2 / PotiMaxWert)* zweiPi;
// Main FOC Algorithm Function
motor_1.loopFOC();
motor_2.loopFOC(); // Loop FOC for Motor 2
// Motion Control Function
motor_1.move(winkel_poti_1);
motor_2.move(winkel_poti_2); // Move Motor 2 to the target angle
// Send Motor Monitoring Data
customMonitor(motor_1,1);
customMonitor(motor_2,2);
// Send Commander Calls
command.run();
}
Thank you so MUCH in Advance!