Commit 68348f60 authored by Torbjørn Ludvigsen's avatar Torbjørn Ludvigsen 👷

Squashed 'firmware/Mechaduino_subtree/' changes from 09a5fab..30d9a5b

30d9a5b Reads encoder to sit still on startup
e2bc136 Lowers PA to aps*0.7
e54516f Removes crazy jerky/reverse motion bug
b8218b4 Fixes indentation and style and removes default lookup table

git-subtree-dir: firmware/Mechaduino_subtree
git-subtree-split: 30d9a5b35cf02a0d297537b87b77612d6c1d9701
parent 8582082c
//Contains TC5 Controller definition
//The main control loop is executed by the TC5 timer interrupt:
// TC5 Controller definition
// The main control loop is executed by the TC5 timer interrupt:
#include <SPI.h>
#include "State.h"
#include "Utils.h"
#include "Parameters.h"
void TC5_Handler() { // gets called with FPID frequency
static int print_counter = 0; //this is used by step response
void TC5_Handler() { // Gets called with FPID frequency
static int print_counter = 0; // This is used by step response
static char prev_mode;
if (TC5->COUNT16.INTFLAG.bit.OVF == 1) { // A counter overflow caused the interrupt
//TEST1_HIGH(); //digitalWrite(3, HIGH); //Fast Write to Digital 3 for debugging
y = lookup[readEncoder()]; //read encoder and lookup corrected angle in calibration lookup table
if ((y - y_1) < -180.0) wrap_count += 1; //Check if we've rotated more than a full revolution (have we "wrapped" around from 359 degrees to 0 or ffrom 0 to 359?)
if (TC5->COUNT16.INTFLAG.bit.OVF == 1) { // A counter overflow caused the interrupt
y = lookup[readEncoder()]; // Read encoder and lookup corrected angle in calibration lookup table
if ((y - y_1) < -180.0) wrap_count += 1; // Check if we've rotated more than a full revolution
else if ((y - y_1) > 180.0) wrap_count -= 1;
yw = (y + (360.0 * wrap_count)); //yw is the wrapped angle (can exceed one revolution)
if (mode == 'h') { //choose control algorithm based on mode
hybridControl(); // hybrid control is still under development...
}
else {
yw = (y + (360.0 * wrap_count)); // yw is the wrapped angle (can exceed one revolution)
if (mode == 'h') { // Choose control algorithm based on mode
hybridControl(); // Hybrid control is still under development...
} else {
switch (mode) {
case 'x': // position control
e = (r - yw);
ITerm += (pKi * e); //Integral wind up limit
if (ITerm > 150.0) ITerm = 150.0;
else if (ITerm < -150.0) ITerm = -150.0;
DTerm = pLPFa*DTerm - pLPFb*pKd*(yw-yw_1);
u = (pKp * e) + ITerm + DTerm;
break;
case 'v': // velocity controlr
v = vLPFa*v + vLPFb*(yw-yw_1); //filtered velocity called "DTerm" because it is similar to derivative action in position loop
e = (r - v); //error in degrees per rpm (sample frequency in Hz * (60 seconds/min) / (360 degrees/rev) )
ITerm += (vKi * e); //Integral wind up limit
case 'x': // Position control
e = (r - yw);
ITerm += (pKi * e); // Integral wind up limit
if (ITerm > 150.0) ITerm = 150.0;
else if (ITerm < -150.0) ITerm = -150.0;
DTerm = pLPFa*DTerm - pLPFb*pKd*(yw-yw_1);
u = (pKp * e) + ITerm + DTerm;
break;
case 'v': // Velocity control
v = vLPFa*v + vLPFb*(yw-yw_1); // Filtered velocity called "DTerm" because it is similar to derivative action in position loop
e = (r - v); // Error in degrees per rpm (sample frequency in Hz * (60 seconds/min) / (360 degrees/rev))
ITerm += (vKi * e); // Integral wind up limit
if (ITerm > 200) ITerm = 200;
else if (ITerm < -200) ITerm = -200;
u = ((vKp * e) + ITerm - (vKd * (e-e_1)));
//SerialUSB.println(e);
break;
case 't': // torque control
case 't': // Torque control
u = 1.0 * torque;
break;
default:
u = 0;
break;
}
y_1 = y; //copy current value of y to previous value (y_1) for next control cycle before PA angle added
if (u > 0) //Depending on direction we want to apply torque, add or subtract a phase angle of PA for max effective torque. PA should be equal to one full step angle: if the excitation angle is the same as the current position, we would not move!
{ //You can experiment with "Phase Advance" by increasing PA when operating at high speeds
y += PA; //update phase excitation angle
if (u > uMAX) // limit control effort
u = uMAX; //saturation limits max current command
}
else
{
y -= PA; //update phase excitation angle
if (u < -uMAX) // limit control effort
u = -uMAX; //saturation limits max current command
y_1 = y; // Copy current value of y to previous value (y_1) for next control cycle before PA angle added
/* Depending on direction we want to apply torque, add or subtract a phase angle of PA for max effective torque.
* PA should be equal to one full step angle: if the excitation angle is the same as the current position, we would not move! */
if (u > 0) { // You can experiment with "Phase Advance" by increasing PA when operating at high speeds
y += PA; // Update phase excitation angle
if (u > uMAX) u = uMAX; // Limit control effort
} else {
y -= PA; // Update phase excitation angle
if (u < -uMAX) u = -uMAX; // Limit control effort
}
U = abs(u); //
if (abs(e) < 0.1) ledPin_HIGH(); // turn on LED if error is less than 0.1
else ledPin_LOW(); //digitalWrite(ledPin, LOW);
output(-y, round(U)); // update phase currents
U = abs(u);
if (abs(e) < 0.1) ledPin_HIGH(); // Turn on LED if error is less than 0.1
else ledPin_LOW();
output(-y, round(U)); // Update phase currents
}
// e_3 = e_2; //copy current values to previous values for next control cycle
e_2 = e_1; //these past values can be useful for more complex controllers/filters. Uncomment as necessary
e_1 = e;
// u_3 = u_2;
u_2 = u_1;
u_1 = u;
yw_1 = yw;
//y_1 = y;
if (print_yw == true){ //for step resonse... still under development
if (print_yw == true) { // For step resonse. still under development
print_counter += 1;
if (print_counter >= 5){ // print position every 5th loop (every time is too much data for plotter and may slow down control loop
SerialUSB.println(int(yw*1024)); //*1024 allows us to print ints instead of floats... may be faster
if (print_counter >= 5) { // Print position every 5th loop (every time is too much data for plotter and may slow down control loop
SerialUSB.println(int(yw*1024)); //*1024 allows us to print ints instead of floats. May be faster
print_counter = 0;
}
}
TC5->COUNT16.INTFLAG.bit.OVF = 1; // writing a one clears the flag ovf flag
//TEST1_LOW(); //for testing the control loop timing
TC5->COUNT16.INTFLAG.bit.OVF = 1; // Writing a one clears the flag ovf flag
}
}
//Contains the TC5 Handler declaration
// TC5 Handler declaration
#ifndef __CONTROLLER_H__
#define __CONTROLLER_H__
#define WAIT_TC16_REGS_SYNC(x) while(x->COUNT16.STATUS.bit.SYNCBUSY);
void TC5_Handler();
#endif
/*
-------------------------------------------------------------
Mechaduino 0.1 & 0.2 Firmware v0.1.4
......@@ -36,9 +35,7 @@
g - generate sine commutation table
m - print main menu
...see serialCheck() in Utils for more details
*/
#include "Utils.h"
......@@ -46,15 +43,7 @@
#include "State.h"
#include "analogFastWrite.h"
//////////////////////////////////////
/////////////////SETUP////////////////
//////////////////////////////////////
void setup() // This code runs once at startup
{
void setup() {
digitalWrite(ledPin,HIGH); // turn LED on
setupPins(); // configure pins
setupTCInterrupts(); // configure controller interrupt
......@@ -66,7 +55,6 @@ void setup() // This code runs once at startup
setupI2C();
digitalWrite(ledPin,LOW); // turn LED off
// Uncomment the below lines as needed for your application.
// Leave commented for initial calibration and tuning.
......@@ -74,25 +62,9 @@ void setup() // This code runs once at startup
// configureEnablePin(); // Active low, for use wath RAMPS 1.4 or similar
enableTCInterrupts(); // uncomment this line to start in closed loop
mode = 'x'; // start in position mode
r = lookup[readEncoder()];
}
//////////////////////////////////////
/////////////////LOOP/////////////////
//////////////////////////////////////
void loop() // main loop
{
void loop() {
serialCheck(); //must have this execute in loop for serial commands to function
//r=0.1125*step_count; //Don't use this anymore. Step interrupts enabled above by "configureStepDir()", adjust step size ("stepangle")in parameters.cpp
}
This source diff could not be displayed because it is too large. You can view the blob instead.
//Contains the Mechaduino parameter declarations
// Mechaduino parameter declarations
#ifndef __PARAMETERS_H__
#define __PARAMETERS_H__
#define firmware_version "0.1.4"
#define identifier "x" // Change this to help keep track of multiple mechaduinos (printed on startup)
#define firmware_version "0.1.4" //firmware version
#define identifier "x" // change this to help keep track of multiple mechaduinos (printed on startup)
//----Current Parameters-----
/* Current Parameters */
extern volatile float Ts;
extern volatile float Fs;
extern volatile float pKp;
extern volatile float pKi;
extern volatile float pKd;
extern volatile float pLPF;
extern volatile float vKp;
extern volatile float vKi;
extern volatile float vKd;
extern volatile float vLPF;
extern const float lookup[];
extern volatile float pLPFa;
extern volatile float pLPFb;
extern volatile float vLPFa;
extern volatile float vLPFb;
extern const int spr; // 200 steps per revolution
extern const float aps; // angle per step
extern int cpr; //counts per rev
extern const int spr; // 200 steps per revolution
extern const float aps; // Angle per step
extern int cpr; // Counts per rev
extern const float stepangle;
extern volatile float PA; //
extern volatile float PA;
extern const float iMAX;
extern const float rSense;
extern volatile int uMAX;
extern const int sin_1[];
//Defines for pins:
/* Pins */
#define IN_4 6
#define IN_3 5
#define VREF_2 4
......@@ -54,18 +39,14 @@ extern const int sin_1[];
#define IN_2 7
#define IN_1 8
#define ledPin 13
#define chipSelectPin A2 //output to chip select
#define chipSelectPin A2 // Output to chip select
#define step_pin 1
#define dir_pin 0
#define enable_pin 2
#define sda_pin 20
#define scl_pin 21
#define I2C_ID 0x0a
//for faster digitalWrite:
/* For faster digitalWrite: */
#define IN_1_HIGH() (REG_PORT_OUTSET0 = PORT_PA06)
#define IN_1_LOW() (REG_PORT_OUTCLR0 = PORT_PA06)
#define IN_2_HIGH() (REG_PORT_OUTSET0 = PORT_PA21)
......@@ -80,26 +61,12 @@ extern const int sin_1[];
#define CHIPSELECT_LOW() (REG_PORT_OUTCLR1 = PORT_PB09)
//#define ENABLE_PROFILE_IO // Define to enable profiling I/O pins
#ifdef ENABLE_PROFILE_IO
#define TEST1 3
#define TEST1_HIGH() (REG_PORT_OUTSET0 = PORT_PA09)
#define TEST1_LOW() (REG_PORT_OUTCLR0 = PORT_PA09)
#else
#define TEST1_HIGH()
#define TEST1_LOW()
#endif
#endif
//Contains the declaration of the state variables for the control loop
//interrupt vars
volatile int U = 0; //control effort (abs)
volatile float r = 0.0; //setpoint
volatile float torque = -40.0; //torque setpoint
volatile float y = 0.0; // measured angle
volatile float v = 0.0; // estimated velocity (velocity loop)
volatile float yw = 0.0; // "wrapped" angle (not limited to 0-360)
// Declarations of control loop state variables
/* Interrupt vars */
volatile int U = 0; // Control effort (abs)
volatile float r = 0.0; // Setpoint
volatile float torque = -40.0; // Torque setpoint
volatile float y = 0.0; // Measured angle
volatile float v = 0.0; // Estimated velocity (velocity mode)
volatile float yw = 0.0; // "wrapped" angle (not limited to 0-360)
volatile float yw_1 = 0.0;
volatile float yw_ref = 0.0;
volatile float e = 0.0; // e = r-y (error)
volatile float p = 0.0; // proportional effort
volatile float i = 0.0; // integral effort
volatile float u = 0.0; //real control effort (not abs)
volatile float u_1 = 0.0; //value of u at previous time step, etc...
volatile float e_1 = 0.0; //these past values can be useful for more complex controllers/filters
volatile float u_2 = 0.0;
volatile float e_2 = 0.0;
volatile float u_3 = 0.0;
volatile float e_3 = 0.0;
volatile float e = 0.0; // e = r-y (error)
volatile float e_1 = 0.0;
volatile float p = 0.0; // Proportional effort
volatile float i = 0.0; // Integral effort
volatile float u = 0.0; // Real control effort (not abs)
volatile float u_1 = 0.0; // Value of u at previous time step, etc...
volatile long counter = 0;
volatile long wrap_count = 0; //keeps track of how many revolutions the motor has gone though (so you can command angles outside of 0-360)
volatile long wrap_count = 0; // Keeps track of how many revolutions the motor has gone though (so you can command angles outside of 0-360)
volatile float y_1 = 0;
volatile long step_count = 0; //For step/dir interrupt (closed loop)
int stepNumber = 0; // open loop step number (used by 's' and for cal routine)
volatile long step_count = 0; // For step/dir interrupt (closed loop)
int stepNumber = 0; // Open loop step number (used by 's' and for cal routine)
volatile float ITerm;
volatile float DTerm;
char mode;
char prev_mode;
volatile bool dir = false;
bool print_yw = false; //for step response, under development...
bool print_yw = false; // For step response, under development...
//Contains the declaration of the state variables for the control loop
// Declarations of state variables for the control loop
#ifndef __STATE_H__
#define __STATE_H__
//interrupt vars
extern volatile int U; //control effort (abs)
extern volatile float r; //setpoint
extern volatile float torque; //setpoint
extern volatile float y; // measured angle
extern volatile float v; // estimated velocity (velocity loop)
/* interrupt vars */
extern volatile int U; // Control effort (abs)
extern volatile float r; // Setpoint
extern volatile float torque; // Setpoint
extern volatile float y; // Measured angle
extern volatile float v; // Estimated velocity (velocity loop)
extern volatile float yw;
extern volatile float yw_ref;
extern volatile float yw_1;
extern volatile float e; // e = r-y (error)
extern volatile float p; // proportional effort
extern volatile float i; // integral effort
extern volatile float u; //real control effort (not abs)
extern volatile float e; // e = r-y (error)
extern volatile float p; // Proportional effort
extern volatile float i; // Integral effort
extern volatile float u; // Real control effort (not abs)
extern volatile float u_1;
extern volatile float e_1;
extern volatile float u_2;
extern volatile float e_2;
extern volatile float u_3;
extern volatile float e_3;
extern volatile long counter;
extern volatile long wrap_count;
extern volatile float y_1;
extern volatile long step_count; //For step/dir interrupt
extern int stepNumber; // step index for cal routine
extern volatile long step_count; // For step/dir interrupt
extern int stepNumber; // Step index for cal routine
extern volatile float ITerm;
extern volatile float DTerm;
extern char mode;
extern int dir;
extern bool print_yw; //for step response, under development...
extern bool print_yw; // For step response, under development...
#endif
This diff is collapsed.
//Contains the declarations for the functions used by the firmware
#ifndef __UTILS_H__
#define __UTIL_H__
void setupPins(); // initializes pins
void setupSPI(); //initializes SPI
void setupI2C(); // initializes I2C
void configureStepDir(); //configure step/dir interface
void configureEnablePin(); //configure enable pin
void stepInterrupt(); //step interrupt handler
void dirInterrupt(); //dir interrupt handler
void enableInterrupt(); //enable pin interrupt handler
void modeChangeInterrupt();
void output(float theta, int effort); //calculates phase currents (commutation) and outputs to Vref pins
void calibrate(); //calibration routine
void serialCheck(); //checks serial port for commands. Must include this in loop() for serial interface to work
void parameterQuery(); //Prints current parameters
void oneStep(void); //take one step
int readEncoder(); //read raw encoder position
void readEncoderDiagnostics(); //check encoder diagnostics registers
void print_angle(); //for debigging purposes in open loop mode: prints [step number] , [encoder reading]
void receiveEvent(int howMany); //for i2c interface...
int mod(int xMod, int mMod); //modulo, handles negative values properly
void setupTCInterrupts(); //configures control loop interrupt
void enableTCInterrupts(); //enables control loop interrupt. Use this to enable "closed-loop" modes
void disableTCInterrupts(); //disables control loop interrupt. Use this to diable "closed-loop" mode
void antiCoggingCal(); //under development...
void parameterEditmain(); //parameter editing menu
void parameterEditp(); //parameter editing menu
void parameterEditv(); //parameter editing menu
void parameterEdito(); //parameter editing menu
void hybridControl(); //open loop stepping, but corrects for missed steps. under development
void serialMenu(); //main menu
void sineGen(); //generates sinusoidal commutation table. you can experiment with other commutation profiles
void stepResponse(); //generates position mode step response in Serial Plotter
void moveRel(float pos_final,int vel_max, int accel); // Generates trapezoidal motion profile for closed loop position mode
void moveAbs(float pos_final,int vel_max, int accel); // Generates trapezoidal motion profile for closed loop position mode
void setupPins(); // Initializes pins
void setupSPI(); // Initializes SPI
void setupI2C(); // Initializes I2C
void configureStepDir(); // Configure step/dir interface
void configureEnablePin(); // Configure enable pin
void stepInterrupt(); // Step interrupt handler
void dirInterrupt(); // Dir interrupt handler
void enableInterrupt(); // Enable pin interrupt handler
void modeChangeInterrupt();
void output(float theta, int effort); // Calculates phase currents (commutation) and outputs to Vref pins
void calibrate(); // Calibration routine
void serialCheck(); // Checks serial port for commands. Must include this in loop() for serial interface to work
void parameterQuery(); // Prints current parameters
void oneStep(void); // Take one step
int readEncoder(); // Read raw encoder position
void readEncoderDiagnostics(); // Check encoder diagnostics registers
void print_angle(); // For debigging purposes in open loop mode: prints [step number] , [encoder reading]
void receiveEvent(int howMany); // For i2c interface...
int mod(int xMod, int mMod); // Modulo, handles negative values properly
void setupTCInterrupts(); // Configures control loop interrupt
void enableTCInterrupts(); // Enables control loop interrupt. Use this to enable "closed-loop" modes
void disableTCInterrupts(); // Disables control loop interrupt. Use this to diable "closed-loop" mode
void antiCoggingCal(); // Under development...
void parameterEditmain(); // Parameter editing menu
void parameterEditp(); // Parameter editing menu
void parameterEditv(); // Parameter editing menu
void parameterEdito(); // Parameter editing menu
void hybridControl(); // Open loop stepping, but corrects for missed steps. under development
void serialMenu(); // Main menu
void sineGen(); // Generates sinusoidal commutation table. you can experiment with other commutation profiles
void stepResponse(); // Generates position mode step response in Serial Plotter
void moveRel(float pos_final,int vel_max, int accel); // Generates trapezoidal motion profile for closed loop position mode
void moveAbs(float pos_final,int vel_max, int accel); // Generates trapezoidal motion profile for closed loop position mode
#endif
......@@ -11,35 +11,31 @@ static int _readResolution = 10;
static int _ADCResolution = 10;
static int _writeResolution = 8;
// Wait for synchronization of registers between the clock domains
/* Wait for synchronization of registers between the clock domains */
static __inline__ void syncADC() __attribute__((always_inline, unused));
static void syncADC() {
while (ADC->STATUS.bit.SYNCBUSY == 1)
;
while (ADC->STATUS.bit.SYNCBUSY == 1);
}
// Wait for synchronization of registers between the clock domains
/* Wait for synchronization of registers between the clock domains */
static __inline__ void syncDAC() __attribute__((always_inline, unused));
static void syncDAC() {
while (DAC->STATUS.bit.SYNCBUSY == 1)
;
while (DAC->STATUS.bit.SYNCBUSY == 1);
}
// Wait for synchronization of registers between the clock domains
/* Wait for synchronization of registers between the clock domains */
static __inline__ void syncTC_8(Tc* TCx) __attribute__((always_inline, unused));
static void syncTC_8(Tc* TCx) {
while (TCx->COUNT8.STATUS.bit.SYNCBUSY);
}
// Wait for synchronization of registers between the clock domains
/* Wait for synchronization of registers between the clock domains */
static __inline__ void syncTCC(Tcc* TCCx) __attribute__((always_inline, unused));
static void syncTCC(Tcc* TCCx) {
while (TCCx->SYNCBUSY.reg & TCC_SYNCBUSY_MASK);
}
static inline uint32_t mapResolution(uint32_t value, uint32_t from, uint32_t to)
{
static inline uint32_t mapResolution(uint32_t value, uint32_t from, uint32_t to) {
if (from == to) {
return value;
}
......@@ -52,64 +48,50 @@ static inline uint32_t mapResolution(uint32_t value, uint32_t from, uint32_t to)
/*
* Internal Reference is at 1.0v
* External Reference should be between 1v and VDDANA-0.6v=2.7v
*
* Warning : On Arduino Zero board the input/output voltage for SAMD21G18 is 3.3 volts maximum
*/
// Right now, PWM output only works on the pins with
// hardware support. These are defined in the appropriate
// pins_*.c file. For the rest of the pins, we default
// to digital output.
void analogFastWrite(uint32_t pin, uint32_t value)
{
/* Right now, PWM output only works on the pins with
* hardware support. These are defined in the appropriate
* pins_*.c file. For the rest of the pins, we default
* to digital output. */
void analogFastWrite(uint32_t pin, uint32_t value) {
PinDescription pinDesc = g_APinDescription[pin];
uint32_t attr = pinDesc.ulPinAttribute;
if ((attr & PIN_ATTR_ANALOG) == PIN_ATTR_ANALOG)
{
// DAC handling code
if ((attr & PIN_ATTR_ANALOG) == PIN_ATTR_ANALOG) {
/* DAC handling code */
if (pin != PIN_A0) { // Only 1 DAC on A0 (PA02)
return;
}
value = mapResolution(value, _writeResolution, 10);
syncDAC();
DAC->DATA.reg = value & 0x3FF; // DAC on 10 bits.
DAC->DATA.reg = value & 0x3FF; // DAC on 10 bits.
syncDAC();
DAC->CTRLA.bit.ENABLE = 0x01; // Enable DAC
DAC->CTRLA.bit.ENABLE = 0x01; // Enable DAC
syncDAC();
return;
}
if ((attr & PIN_ATTR_PWM) == PIN_ATTR_PWM)
{
if ((attr & PIN_ATTR_PWM) == PIN_ATTR_PWM) {
value = mapResolution(value, _writeResolution, 8); // change to 10 for 10 bit... must also change TCx->COUNT8.PER.reg = 0x3FF
uint32_t tcNum = GetTCNumber(pinDesc.ulPWMChannel);
uint8_t tcChannel = GetTCChannelNumber(pinDesc.ulPWMChannel);
static bool tcEnabled[TCC_INST_NUM+TC_INST_NUM];
if (!tcEnabled[tcNum]) {
tcEnabled[tcNum] = true;
if (attr & PIN_ATTR_TIMER) {
#if !(ARDUINO_SAMD_VARIANT_COMPLIANCE >= 10603)
// Compatibility for cores based on SAMD core <=1.6.2
#if !(ARDUINO_SAMD_VARIANT_COMPLIANCE >= 10603)
/* Compatibility for cores based on SAMD core <=1.6.2 */
if (pinDesc.ulPinType == PIO_TIMER_ALT) {
pinPeripheral(pin, PIO_TIMER_ALT);
} else
#endif
#endif
{
pinPeripheral(pin, PIO_TIMER);
}
} else {
// We suppose that attr has PIN_ATTR_TIMER_ALT bit set...
/* We suppose that attr has PIN_ATTR_TIMER_ALT bit set... */
pinPeripheral(pin, PIO_TIMER_ALT);
}
uint16_t GCLK_CLKCTRL_IDs[] = {
GCLK_CLKCTRL_ID(GCM_TCC0_TCC1), // TCC0
GCLK_CLKCTRL_ID(GCM_TCC0_TCC1), // TCC1
......@@ -122,44 +104,31 @@ void analogFastWrite(uint32_t pin, uint32_t value)
};
GCLK->CLKCTRL.reg = (uint16_t) (GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK0 | GCLK_CLKCTRL_IDs[tcNum]);
while (GCLK->STATUS.bit.SYNCBUSY == 1);