-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Deployed 5d62d73 with MkDocs version: 1.5.3
- Loading branch information
Unknown
committed
Oct 6, 2023
0 parents
commit 08190e0
Showing
155 changed files
with
42,147 additions
and
0 deletions.
There are no files selected for viewing
Empty file.
Large diffs are not rendered by default.
Oops, something went wrong.
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
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,76 @@ | ||
// Open loop motor control example | ||
#include <SimpleFOC.h> | ||
|
||
|
||
// BLDC motor & driver instance | ||
// BLDCMotor motor = BLDCMotor(pole pair number); | ||
BLDCMotor motor = BLDCMotor(7); | ||
// BLDCDriver3PWM driver = BLDCDriver3PWM(pwmA, pwmB, pwmC, Enable(optional)); | ||
BLDCDriver6PWM driver = BLDCDriver6PWM(5, 6, 9,10, 3, 11); | ||
|
||
// Stepper motor & driver instance | ||
//StepperMotor motor = StepperMotor(50); | ||
//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8); | ||
|
||
|
||
//target variable | ||
float target_velocity = 6; | ||
|
||
// // instantiate the commander | ||
Commander command = Commander(Serial); | ||
// void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } | ||
// void doLimit(char* cmd) { command.scalar(&motor.voltage_limit, cmd); } | ||
|
||
void setup() { | ||
|
||
// driver config | ||
// power supply voltage [V] | ||
driver.voltage_power_supply = 5; | ||
// limit the maximal dc voltage the driver can set | ||
// as a protection measure for the low-resistance motors | ||
// this value is fixed on startup | ||
driver.voltage_limit = 5; | ||
// pwm frequency to be used [Hz] | ||
// for atmega328 fixed to 32kHz | ||
// esp32/stm32/teensy configurable | ||
driver.pwm_frequency = 32000; | ||
|
||
|
||
driver.init(); | ||
// link the motor and the driver | ||
motor.linkDriver(&driver); | ||
|
||
// limiting motor movements | ||
// limit the voltage to be set to the motor | ||
// start very low for high resistance motors | ||
// current = voltage / resistance, so try to be well under 1Amp | ||
motor.voltage_limit = 3; // [V] | ||
|
||
// open loop control config | ||
motor.controller = MotionControlType::velocity_openloop; | ||
|
||
// init motor hardware | ||
motor.init(); | ||
|
||
// add target command T | ||
// command.add('T', doTarget, "target velocity"); | ||
// command.add('L', doLimit, "voltage limit"); | ||
|
||
Serial.begin(115200); | ||
Serial.println("Motor ready!"); | ||
Serial.println("Set target velocity [rad/s]"); | ||
_delay(1000); | ||
|
||
} | ||
|
||
void loop() { | ||
|
||
// open loop velocity movement | ||
// using motor.voltage_limit and motor.velocity_limit | ||
motor.move(target_velocity); | ||
|
||
// user communication | ||
command.run(); | ||
|
||
|
||
} |
135 changes: 135 additions & 0 deletions
135
assets/arduino_examples/IoT_MotorDriver/IoT_MotorDriver.ino
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
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,135 @@ | ||
/****************************************************************************** | ||
IoT Motor Driver Example | ||
Written By: | ||
Madison Chodikov | ||
Eric Orosel | ||
Company: SparkFun Electronics | ||
Date: September 1 2023 | ||
This sketch is a stripped down version of the firmware that is preprogrammed | ||
on the IoT Motor Driver. It is based on the open loop, velocity motor control | ||
example from the SimpleFOC Arduino library. | ||
This sketch will spin the motor based on the button inputs: | ||
- Button 13: Starts/Stops the motor rotation | ||
- Button 14: When spinning, switches the direction of rotation | ||
=============================================================================== | ||
Products: | ||
IoT Brushless Motor Driver: https://www.sparkfun.com/products/22132 | ||
Repository: | ||
https://github.com/sparkfun/SparkFun_IoT_Brushless_Motor_Driver | ||
=============================================================================== | ||
SparkFun code, firmware, and software is released under the MIT | ||
License (http://opensource.org/licenses/MIT). | ||
Distributed as-is; no warranty is given. | ||
******************************************************************************/ | ||
|
||
#include <Wire.h> | ||
#include <SimpleFOC.h> //http://librarymanager/All#Simple%20FOC | ||
|
||
|
||
//GPIO | ||
#define auxBtn2 13 | ||
#define auxBtn1 14 | ||
|
||
//driver | ||
#define uh16 16 | ||
#define ul17 17 | ||
#define vh18 18 | ||
#define wh19 19 | ||
#define vl23 23 | ||
#define wl33 33 | ||
#define curSense 32 | ||
|
||
bool state = true; | ||
|
||
//motor driver | ||
BLDCMotor motor = BLDCMotor(7); | ||
BLDCDriver6PWM driver = BLDCDriver6PWM(uh16, ul17, vh18, vl23, wh19, wl33, curSense); | ||
float target_velocity = 0.0; | ||
Commander command = Commander(Serial); | ||
void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } | ||
void doLimit(char* cmd) { command.scalar(&motor.voltage_limit, cmd); } | ||
|
||
//////////////////////motor demo stuff/////////////////////////// | ||
struct Button{ | ||
const uint8_t PIN; | ||
uint32_t numberKeyPresses; | ||
bool pressed; | ||
}; | ||
Button aux1 = {auxBtn1, 0, false}; | ||
Button aux2 = {auxBtn2, 0, false}; | ||
|
||
void IRAM_ATTR isr1(){ | ||
aux1.pressed = true; | ||
target_velocity = target_velocity*(-1); | ||
Serial.println("Changing directions.. "); | ||
} | ||
|
||
void IRAM_ATTR isr2(){ | ||
aux2.numberKeyPresses++; | ||
aux2.pressed = true; | ||
|
||
if((aux2.numberKeyPresses % 2) == 0) | ||
{ | ||
target_velocity = 0; | ||
Serial.println("Stopping motor.. "); | ||
} | ||
else | ||
{ | ||
target_velocity = 5; | ||
motor.enable(); | ||
Serial.println("Starting motor.. "); | ||
} | ||
} | ||
|
||
|
||
void setup() { | ||
|
||
//motor demo stuff | ||
driver.voltage_power_supply = 3.3; | ||
driver.pwm_frequency = 20000; | ||
driver.voltage_limit = 4; | ||
driver.init(); | ||
motor.linkDriver(&driver); | ||
motor.voltage_limit = 4; | ||
motor.controller = MotionControlType::velocity_openloop; | ||
motor.init(); | ||
motor.disable(); | ||
pinMode(aux1.PIN, INPUT_PULLUP); // Sets pin 14 on the ESP32 as an interrupt | ||
attachInterrupt(aux1.PIN, isr1, FALLING); // Triggers when aux1 is pulled to GND (button pressed) | ||
pinMode(aux2.PIN, INPUT_PULLUP); // Sets pin 13 on the ESP32 as an interrupt | ||
attachInterrupt(aux2.PIN, isr2, FALLING); // Triggers when aux2 is pulled to GND (button pressed) | ||
delay(100); | ||
|
||
Serial.begin(115200); | ||
} | ||
|
||
///////////////////////////////////////////////////////////////////////// | ||
void loop() { | ||
|
||
// Button Press ISR | ||
if(aux1.pressed){ | ||
aux1.pressed = false; | ||
} | ||
|
||
// Turning motor on and off | ||
if(aux2.pressed){ | ||
aux2.pressed = false; | ||
} | ||
|
||
// open loop velocity movement | ||
// using motor.voltage_limit and motor.velocity_limit | ||
// Basic motor movement | ||
motor.move(target_velocity); | ||
|
||
// user communication | ||
command.run(); | ||
|
||
delay(5); | ||
} |
Binary file not shown.
Binary file not shown.
Oops, something went wrong.