here is my code:
#include <SimpleFOC.h>
// #include <PciManager.h>
// #include <PciListenerImp.h>
#define USESENSOR (HALLSENSOR)
#define pp 15
//motor1
//motor2
#define INH_A2 45 //PWM_A
#define INH_B2 48 //PWM_B
#define INH_C2 47 //PWM_C
#define EN_GATE2 35
#define M_PWM2 38
#define M_OC2 37
#define OC_ADJ2 36
#define HALLa2 (17)
#define HALLb2 (8)
#define HALLc2 (3)
// Motor instance
BLDCMotor motor2 = BLDCMotor(pp);
// BLDCDriver3PWM(IN1, IN2, IN3, enable(optional))
//BLDCDriver3PWM driver = BLDCDriver3PWM(PB6, PB7, PB8, PB5);
BLDCDriver3PWM driver2 = BLDCDriver3PWM(INH_A2, INH_B2, INH_C2, EN_GATE2);
// hallsensor instance
HallSensor hsensor2 = HallSensor(HALLa2, HALLb2, HALLc2, pp);
// channel A and B callbacks
void doA2(){hsensor2.handleA();}
void doB2(){hsensor2.handleB();}
void doC2(){hsensor2.handleC();}
//PciListenerImp listenC(hsensor.pinC, doC);
// angle set point variable
//float target_angle = 0;
float target_velocity1 = 0.0;
float target2 = 0.0;
// instantiate the commander
Commander command = Commander(Serial);
void doTarget2(char* cmd2) { command.scalar(&target2, cmd2); }
void setup() {
//motor1
pinMode(M_OC1,OUTPUT);
digitalWrite(M_OC1,LOW);
// M_PWM - enable 3pwm mode
pinMode(M_PWM1,OUTPUT);
digitalWrite(M_PWM1,HIGH);
// OC_ADJ - set the maximum overcurrent limit possible
// Better option would be to use voltage divisor to set exact value
pinMode(OC_ADJ1,OUTPUT);
digitalWrite(OC_ADJ1,HIGH);
//motor2
pinMode(M_OC2,OUTPUT);
digitalWrite(M_OC2,LOW);
// M_PWM - enable 3pwm mode
pinMode(M_PWM2,OUTPUT);
digitalWrite(M_PWM2,HIGH);
// OC_ADJ - set the maximum overcurrent limit possible
// Better option would be to use voltage divisor to set exact value
pinMode(OC_ADJ2,OUTPUT);
digitalWrite(OC_ADJ2,HIGH);
// initialize sensor hardware
// check if you need internal pullups
hsensor2.pullup = Pullup::USE_EXTERN;
hsensor2.init();
// hardware interrupt enable
hsensor2.enableInterrupts(doA2, doB2, doC2);
// link the motor to the sensor
motor2.linkSensor(&hsensor2);
// driver config
// power supply voltage [V]
driver2.pwm_frequency = 20000;
//driver1.voltage_power_supply = 24;
//driver1.voltage_limit = 12;
driver2.voltage_power_supply = 36;
//driver2.voltage_limit = 12;
driver2.init();
motor2.linkDriver(&driver2);
// link the motor and the driver
// aligning voltage [V]
// motor1.voltage_sensor_align = 3;
motor2.voltage_sensor_align = 3;
// motor2.torque_controller = TorqueControlType::voltage;
motor2.controller = MotionControlType::velocity;
// contoller configuration
// default parameters in defaults.h
// // velocity PI controller parameters
motor2.PID_velocity.P = 0.01;
motor2.PID_velocity.I = 0;
//motor2.PID_velocity.D = 0.01;
// the lower the less filtered
motor2.LPF_velocity.Tf = 0.08;
//motor.PID_velocity.limit = 0;
// motor.PID_velocity.I = 50;
// default voltage_power_supply
//motor1.voltage_limit = 2;
motor2.voltage_limit = 4;
motor2.velocity_limit = 40; //rad/s
// use monitoring with serial
Serial.begin(115200);
// comment out if not needed
motor2.useMonitoring(Serial);
//motor1.monitor_downsample = 30; // default 10
motor2.monitor_downsample = 50; // default 10
// initialize motor
motor1.init();
motor2.init();
// align encoder and start FOC
motor2.zero_electric_angle = 0.00; // rad
motor2.sensor_direction = Direction::CW; // CW or CCW
//motor1.initFOC();
motor2.initFOC();
// Serial.println(motor2.zero_electric_angle);
// add target command T
command.add(‘t’, doTarget1, “target angle”);
command.add(‘u’, doTarget2, “target angle”);
Serial.println(F(“Motor ready.”));
Serial.println(F(“Set the target angle using serial terminal:”));
_delay(1000);
}
void loop() {
/
//Serial.print(hsensor.getAngle());
//Serial.print(“\t”);
//motor1.loopFOC();
motor2.PID_velocity.I = target2;
motor2.loopFOC();
motor2.move(5);
//motor1.monitor();
motor2.monitor();
// Serial.print(hsensor2.getAngle());
command.run();
//delay(100);
}
