Skip to content

Instantly share code, notes, and snippets.

@jschoch
Last active March 27, 2021 18:12
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save jschoch/f400c147a418671a2d97a0c138d00983 to your computer and use it in GitHub Desktop.
Save jschoch/f400c147a418671a2d97a0c138d00983 to your computer and use it in GitHub Desktop.
simple foc pendulum
#include <Arduino.h>
#include <SimpleFOC.h>
#include <neotimer.h>
int led = 13;
int d = 200;
BLDCMotor motor = BLDCMotor(7);
BLDCDriver3PWM driver = BLDCDriver3PWM(25, 26, 27, 18);
//Pullup pullup = Pullup::INTERN;
Neotimer st = Neotimer(100);
float target_velocity = 30.0;
Encoder encoder = Encoder(15,17, 600);
Commander command = Commander(Serial);
// angle set point variable
float target_angle = 0;
void doTarget(char* cmd) { command.scalar(&target_angle, cmd); }
void onMotor(char* cmd){ command.motor(&motor, cmd); }
//void onPid(char* cmd){ commander.pid(&pid,cmd); }
void doA(){encoder.handleA();}
void doB(){encoder.handleB();}
const float pi = 3.14;
float length = 28;
//float pangle = 85*(pi/180); // radians
float pangle = .7;
float angleAccel = 0.0;
float angleVelocity = 0.0;
// time interval?
// smaller == slower
float dt = 0.2;
Neotimer angle_timer = Neotimer(20);
Neotimer angle_printer = Neotimer(1000);
float grav = -9.81;
float newAngle = 0.0;
int counter = 0;
bool startup = true;
float vomit = true;
void doLength(char* cmd){command.scalar(&length,cmd);};
void doVomit(char* cmd){ command.scalar(&vomit,cmd);}
void getAngle(){
angleAccel = grav / length * sin(pangle);
angleVelocity += angleAccel * dt;
pangle += angleVelocity * dt;
if(angle_printer.repeat() || vomit){
Serial.println(newAngle);
}
newAngle = pangle;
}
void setup(){
encoder.init();
encoder.enableInterrupts(doA, doB);
motor.linkSensor(&encoder);
Serial.begin(115200);
driver.voltage_power_supply = 24;
driver.init();
// link the motor and the driver
motor.linkDriver(&driver);
// aligning voltage [V]
motor.voltage_sensor_align = 5;
// index search velocity [rad/s]
motor.velocity_index_search = 5;
// set motion control loop to be used
motor.controller = MotionControlType::angle;
//motor.controller = ControlType::velocity_openloop;
// contoller configuration
// default parameters in defaults.h
// velocity PI controller parameters
motor.PID_velocity.P = 0.5;
motor.PID_velocity.I = 1.0;
// default voltage_power_supply
motor.voltage_limit = 24;
// jerk control using voltage voltage ramp
// default value is 300 volts per sec ~ 0.3V per millisecond
motor.PID_velocity.output_ramp = 900;
// velocity low pass filtering time constant
motor.LPF_velocity.Tf = 0.01;
// angle P controller
motor.P_angle.P = 30;
// maximal velocity of the position control
motor.velocity_limit = 10;
// comment out if not needed
motor.useMonitoring(Serial);
// initialize motor
motor.init();
// align encoder and start FOC
motor.initFOC();
// add target command T
command.add('T', doTarget, "target angle");
command.add('L', doLength, "pendulum length");
command.add('A', onMotor, "motor");
command.add('P', doVomit, "vomit");
//command.add('C',onPid,"my pid");
Serial.println("Motor ready.");
Serial.println("Set the target angle using serial terminal:");
_delay(1000);
command.verbose = VerboseMode::user_friendly;
motor.move(pangle);
while(startup){
motor.loopFOC();
counter++;
if(counter > 10000){
startup=false;
}
}
}
void loop(){
motor.loopFOC();
if(angle_timer.repeat()){
getAngle();
target_angle = newAngle;
}
motor.move(target_angle);
command.run();
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment