Last active
March 27, 2021 18:12
-
-
Save jschoch/f400c147a418671a2d97a0c138d00983 to your computer and use it in GitHub Desktop.
simple foc pendulum
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#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