Post by whitelion on Jan 18, 2022 8:34:34 GMT
To Whom It May Concern,
I am having trouble recompiling a sketch after having updated the Aurix Free Toolchain, Arduino IDE and ShieldBuddy IDE along with using a new license. Specifically I am using the string.h library to convert ASCII characters to a floating point number. The entire sketch is provided below but the relevant code is just:
#include <string.h>
.
.
.
inString = String(fip_cup);
*UFDC_sig = inString.toFloat();
Compile error is:
sketch\ShieldBuddyBalanceBeamSketch_v2.ino.cpp.o: In function `read_UFDC(float*, int)':
ShieldBuddyBalanceBeamSketch_v2:812: error: undefined reference to `String::operator=(String&&)'
collect2.exe: error: ld returned 1 exit status
exit status 1
undefined reference to `String::operator=(String&&)'
This code has compiled before without errors but the recent updates may have done some damage. Pls help ;-) Also I should mention that this linker/compile error does not happen when I compile for the Arduino Due target which is strange.
-Joel
/*** Don't worry, the normal Arduino setup() and loop() are below this block! ***/
#include "MegunoLink.h"
#include "CommandHandler.h"
#include <math.h>
#include <SPI.h>
//#include <SpiIf.h>
#include <Metro.h>
#include <stdio.h>
#include <string.h>
#define Serial SerialASC
#define M_PI 3.1415
Metro ISRMetroCore0 = Metro(200); // 100 ms
Metro ISRMetroCore1 = Metro(5); // 5 ms
Metro ISRMetroCore2 = Metro(1000); // 1000 ms
const unsigned int D2A_SCK_RATE = 500000; /* 10MHz, 10000000 */
const unsigned int UFDC_SCK_RATE = 500000; // 500000 100000 /* 100 - 500 kHz */
const unsigned int A2D_SCK_RATE = 500000; /* 500000 100000 1000000 5MHz */
SPISettings myD2ASettings(D2A_SCK_RATE, MSBFIRST, SPI_MODE0);
SPISettings myUFDCSettings(UFDC_SCK_RATE, MSBFIRST, SPI_MODE3); // SPI_MODE3
SPISettings myA2DSettings(A2D_SCK_RATE, MSBFIRST, SPI_MODE1); // SPI_MODE0 SPI_MODE1 for ShieldBuddy
float convert_thermistor_voltage2temp(int);
void read_UFDC(float *, int);
void convert_tank_periods2height(float *, float *, float, float, float, float, float, float, float, float, float);
void send_D2A_voltages(float *, float *);
float convert_hallpot_voltage2angle(float, float, float);
float read_A2D_voltage();
void MessageLogger_CSVData(long, float, float, float, float, float, float, float, float, float, float, float, String);
float estimate_angular_rate(float);
void do_pump_control(float *, float *, float, float, float);
void do_beam_control(float *, float, float, float);
void do_reset_state();
CommandHandler<> SerialCommandHandler(SerialASC);
TimePlot AnglePlot("Beam Angle",SerialASC); // "Channel name", appears on plot tab and drop down menu
TimePlot AngularRatePlot("Beam Angular Rate",SerialASC);
TimePlot WaterLevelPlot("Water Levels",SerialASC);
TimePlot TemperaturePlot("Water Temperature",SerialASC);
TimePlot ControlInputPlot("Control Voltages",SerialASC);
/* LMU uninitialised data */
StartOfUninitialised_LMURam_Variables
/* Put your LMU RAM fast access variables that have no initial values here e.g. uint32 LMU_var; */
EndOfUninitialised_LMURam_Variables
/* LMU uninitialised data */
StartOfInitialised_LMURam_Variables
/* Put your LMU RAM fast access variables that have an initial value here e.g. uint32 LMU_var_init = 1; */
float water_temperature_average_shr = 0.0;
float hallpot_voltage_shr = 0.0;
float theta_shr = 0.0;
float theta_dot_est_shr = 0.0;
float UFDC_sig_right_shr = 0.0;
float UFDC_sig_left_shr = 0.0;
float water_height_right_shr = 0.0;
float water_height_left_shr = 0.0;
float delta_h_shr = 0.0;
float input_Vr_shr = 0.0;
float input_Vl_shr = 0.0;
float PumpIdleVoltageRight_shr = 0.0;
float PumpIdleVoltageLeft_shr = 0.0;
boolean PUMP_CONTROL_STATE_shr = false;
boolean BEAM_CONTROL_STATE_shr = false;
boolean RESET_STATE_shr = false;
float delta_h_ref_pc_shr = 0.0;
float theta_ref_shr = 0.0;
float kp_shr = 50;
EndOfInitialised_LMURam_Variables
/* If you do not care where variables end up, declare them here! */
/*** Core 0 ***/
const int CORE0HB = 2;
int CORE0HB_flag = 0;
const int core0_timing_pin = 51;
unsigned long timer;
unsigned long timer_setup;
float PumpIdleVoltageRight = 0.0;
float PumpIdleVoltageLeft = 0.0;
boolean PUMP_CONTROL_STATE = false;
boolean BEAM_CONTROL_STATE = false;
boolean RESET_STATE = false;
float delta_h_ref_pc = 0.0;
float theta_ref = 0.0;
float kp = 50.0;
void setup() {
// put your setup code for core 0 here, to run once:
pinMode(CORE0HB, OUTPUT);
digitalWrite(CORE0HB,HIGH); // start out high
pinMode(core0_timing_pin,OUTPUT);
digitalWrite(core0_timing_pin,HIGH);
Serial.begin(115200);
Serial.println("\n");
Serial.println("\n");
Serial.println("\n");
Serial.println("*************************************************************");
Serial.println("********** Starting Three Core Balance Beam Sketch **********");
Serial.println("*************************************************************");
Serial.println("\n");
Serial.println("Serial Connection Opened At 115200 Baud Rate On Core 0 ...");
Serial.println("\n");
AnglePlot.SetTitle("Hallpot Angle Measurement");
AnglePlot.SetXlabel("Time (sec.)");
AnglePlot.SetYlabel("Angle (deg.)");
AnglePlot.SetSeriesProperties("Calibrated Hallpot Angle", TimePlot::Red, TimePlot::Solid, 3, TimePlot::Plus); // legend tag
AngularRatePlot.SetTitle("Estimated Angular Rate");
AngularRatePlot.SetXlabel("Time (sec.)");
AngularRatePlot.SetYlabel("Angular Rate (deg./sec.)");
AngularRatePlot.SetSeriesProperties("Estimated Angular Rate", TimePlot::Red, TimePlot::Solid, 3, TimePlot::Plus);
WaterLevelPlot.SetTitle("Water Level Measurements");
WaterLevelPlot.SetXlabel("Time (sec.)");
WaterLevelPlot.SetYlabel("Height (m)");
WaterLevelPlot.SetSeriesProperties("Calibrated Water Level (Right - Left)", TimePlot::Blue, TimePlot::Solid, 3, TimePlot::Plus);
TemperaturePlot.SetTitle("Temperature Measurements");
TemperaturePlot.SetXlabel("Time (sec.)");
TemperaturePlot.SetYlabel("Temperature (deg. C)");
TemperaturePlot.SetSeriesProperties("Calibrated Water Temperature (Average)", TimePlot::Green, TimePlot::Solid, 3, TimePlot::Plus);
ControlInputPlot.SetTitle("Control Input Voltages");
ControlInputPlot.SetXlabel("Time (sec.)");
ControlInputPlot.SetYlabel("Voltage (V)");
ControlInputPlot.SetSeriesProperties("Right Pump Voltage", TimePlot::Magenta, TimePlot::Solid, 3, TimePlot::Plus);
ControlInputPlot.SetSeriesProperties("Left Pump Voltage", TimePlot::Cyan, TimePlot::Solid, 3, TimePlot::Plus);
SerialCommandHandler.AddVariable(F("PumpIdleVoltageRight"), PumpIdleVoltageRight);
SerialCommandHandler.AddVariable(F("PumpIdleVoltageLeft"), PumpIdleVoltageLeft);
SerialCommandHandler.SetDefaultHandler(Cmd_Unknown);
SerialCommandHandler.AddCommand(F("WHCCheckBoxEvent"), Cmd_WHCCheckBoxEvent);
SerialCommandHandler.AddCommand(F("BACCheckBoxEvent"), Cmd_BACCheckBoxEvent);
SerialCommandHandler.AddCommand(F("ZeroFBCStates"), Cmd_ZeroStates);
SerialCommandHandler.AddVariable(F("ReferenceWaterHeight"), delta_h_ref_pc);
SerialCommandHandler.AddVariable(F("ReferenceBeamAngle"), theta_ref);
SerialCommandHandler.AddVariable(F("Kp"), kp);
timer_setup = millis();
}
void loop() {
SerialCommandHandler.Process();
// put your main code for core 0 here, to run repeatedly:
if (ISRMetroCore0.check() == 1)
{
digitalWrite(core0_timing_pin,LOW);
AnglePlot.SendFloatData("Calibrated Hallpot Angle", theta_shr*(180.0/M_PI), 5); // "Series name"
AngularRatePlot.SendFloatData("Estimated Angular Rate", theta_dot_est_shr*(180.0/M_PI), 4);
WaterLevelPlot.SendFloatData("Calibrated Water Level (Right - Left)", delta_h_shr, 4); // delta_h
TemperaturePlot.SendFloatData("Calibrated Water Temperature (Average)", water_temperature_average_shr, 4); // water_temperature_average
ControlInputPlot.SendFloatData("Right Pump Voltage", input_Vr_shr, 4);
ControlInputPlot.SendFloatData("Left Pump Voltage", input_Vl_shr, 4);
timer = millis() - timer_setup;
MessageLogger_CSVData(timer, water_temperature_average_shr, hallpot_voltage_shr, theta_shr, theta_dot_est_shr, UFDC_sig_right_shr, UFDC_sig_left_shr, water_height_right_shr, water_height_left_shr, delta_h_shr, input_Vr_shr, input_Vl_shr, "Sensor_Actuator_Data");
// Transfer data to shared LMU memory
PumpIdleVoltageRight_shr = PumpIdleVoltageRight;
PumpIdleVoltageLeft_shr = PumpIdleVoltageLeft;
PUMP_CONTROL_STATE_shr = PUMP_CONTROL_STATE;
BEAM_CONTROL_STATE_shr = BEAM_CONTROL_STATE;
RESET_STATE_shr = RESET_STATE;
delta_h_ref_pc_shr = delta_h_ref_pc;
theta_ref_shr = theta_ref;
kp_shr = kp;
RESET_STATE = false;
CORE0HB_flag = !CORE0HB_flag;
digitalWrite(CORE0HB,CORE0HB_flag);
digitalWrite(core0_timing_pin,HIGH);
}
}
/*** Core 1 ***/
/* CPU1 Uninitialised Data */
StartOfUninitialised_CPU1_Variables
/* Put your CPU1 fast access variables that have no initial values here e.g. uint32 CPU1_var; */
EndOfUninitialised_CPU1_Variables
/* CPU1 Initialised Data */
StartOfInitialised_CPU1_Variables
/* Put your CPU1 fast access variables that have an initial value here e.g. uint32 CPU1_var_init = 1; */
const int CORE1HB = 22;
int CORE1HB_flag = 0;
const int core1_timing_pin = 53;
// Thermistor parameters
float MAX_V = 5.0; // 3.3 Reference voltage for A2D channels with Due. 5.0 with ShieldBuddy
int MAX_A2D = 1023;
int analogPin0 = 0;
int analogPin1 = 1;
float THETA[3] = {3.33746911687670e+2,-3.83716062783759e+1,5.73463936130444e-2};
float MIN_RESISTANCE = 303.075;
float MAX_RESISTANCE = 367701.4;
float divider_resistance = 10000; // Ohms
// Thermistor variables
int twelve_bit_A2D0_val = 0;
int twelve_bit_A2D1_val = 0;
float water_temperature_right = 0.0;
float water_temperature_left = 0.0;
float water_temperature_average = 0.0;
// Hallpot parameters
float m_v2rad = 4.439744921303501e-01; // 4.507506390723722e-01; 25.2643013874206; -16.7918701445676;
float b_rad = -1.106880081759474; // -1.110281348181693; -59.4501610744572; 39.1664878889829;
const int SBBTC = 4; // SBBTC, straight binary or binary twos complement, output (HIGH for SB on pin 8 of AD977)
const int EXTINT = 5; // EXTINT external or internal clock, output (HIGH for external on pin 9 of AD977)
const int RC = 6; // RC, read/convert, output
const int BUSY = 3; // BUSY, input
const int PWRD = 7; // PWRD, power down, output (LOW for powered up on pin 18 of AD977)
const int A2D_CS = 11; // A2D CS, output
const unsigned short MAX_word = 65535; /* 2^16 - 1 = 65535 is maximum unsigned short */
const float MAX_A2D_voltage = 5.0;
// Hallpot variables
float hallpot_voltage = 0.0;
float theta = 0.0;
float theta_dot_est = 0.0;
// Water tank parameters
float m_p2m_right = 5.912248633713175e-04;
float b_m_right = -1.279781180930623e-01;
float m_rad2p_right = 4.107996159153647;
float m_p2m_left = 5.642928988291027e-04;
float b_m_left = -1.267508779208012e-01;
float m_rad2p_left = -2.931498670086657;
const int UFDC_CS_right = 12; // 12 UFDC CS output (right tank)
const int UFDC_CS_left = 8; // 13 UFDC CS output (left tank)
// Water tank variables
float UFDC_sig_right = 100.0;
float UFDC_sig_left = 200.0;
float water_height_right = 0.0;
float water_height_left = 0.0;
float delta_h = 0.0; // right - left
// D2A parameters
const int D2A1_CS = 9; /* D2A chip selects */
const int D2A2_CS = 10;
const float MAX_D2A_voltage = 5.0;
const float MIN_D2A_voltage = 0.0;
// D2A variables
float input_Vr = 0.0;
float input_Vl = 0.0;
float zero = 0.0;
// Rate estimator filter parameters
#define Nre 2
//float Are[Nre][Nre] = {{-1.110223024625157e-16,9.906194960589272e-01},{-1.000000000000000e+00,1.988409675872070e+00}}; // 1.5 Hz corner
//float Bre[Nre] = {-2.513258523646844,-4.648541838448669e-01};
//float Cre[Nre] = {0.0,-1.0};
//float Dre[1] = {0.0};
float Are[Nre][Nre] = {{-2.220446049250313e-16,9.782488939094772e-01},{-1.000000000000000e+00,1.966302642061996e+00}}; // 3.5 Hz corner
float Bre[Nre] = {-2.513258523646844,-2.513258523646843};
float Cre[Nre] = {0.0,-1.0};
float Dre[1] = {0.0};
float xk_re[Nre] = {0.0,0.0};
// Pump control variables
float delta_h_ref_total = 0.0;
float delta_h_ref_bc = 0.0;
float pump_control_voltage_right = 0.0;
float pump_control_voltage_left = 0.0;
// Beam control filters
//#define Nbm 7
//float Abm[Nbm][Nbm] = {{-6.453171330633722e-16,-3.592757680011890e-16,1.737283960164751e-16,1.779994139821527e-15,4.253745440190021e-16,-8.448260360106643e-15,-1.970287298659054e-05},
// {1.000000000000002e-01,-4.600194883798486e-16,-1.263565400178538e-16,-8.268511417045545e-16,-1.976873611380493e-15,-1.010096454786383e-16,2.311104748464772e-05},
// {0.000000000000000e+00,1.000000000000001e+00,6.330127011466199e-16,1.134069082291906e-15,3.170283497285367e-16,1.051300413025089e-16,-1.110555110601209e-04},
// {0.000000000000000e+00,0.000000000000000e+00,-9.999999999999994e-01,6.153468078650685e-16,2.377423854111574e-16,6.441672352387496e-17,-2.952827105707959e-04},
// {0.000000000000000e+00,0.000000000000000e+00,0.000000000000000e+00,9.999999999999998e+00,-5.544184936907761e-15,-8.553179697172885e-16,4.931606683667007e-03},
// {0.000000000000000e+00,0.000000000000000e+00,0.000000000000000e+00,0.000000000000000e+00,1.000000000000000e+01,1.848798365247152e-15,-5.253664833940819e-02},
// {0.000000000000000e+00,0.000000000000000e+00,0.000000000000000e+00,0.000000000000000e+00,0.000000000000000e+00,1.000000000000000e+02,3.375737747240404e+00}}; // 10.0 Hz BW
//float Bbm[Nbm] = {-4.772120388909852e-01,2.960723091657326e-01,-7.835695530322973e-01,-1.128468652061411e+00,9.288999636979765e+00,-4.126379379248117e+01,7.698817900722686e+02};
//float Cbm[Nbm] = {0.0,0.0,0.0,0.0,0.0,0.0,1000.0};
//float Dbm[1] = {0.0};
//float xk_bm[Nbm] = {0.0,0.0,0.0,0.0,0.0,0.0,0.0};
#define Nbm 4
float Abm[Nbm][Nbm] = {{-4.440892098500626e-16,9.109600434118143e-17,-6.670338036882551e-16,8.334243240386791e-02},
{1.000000000000000e+00,2.024977149732429e-16,3.642692133582339e-16,-3.493334277882195e-01},
{0.000000000000000e+00,-1.000000000000000e+00,-5.915248990139340e-16,-5.485829047470054e-01},
{0.000000000000000e+00,0.000000000000000e+00,1.000000000000000e+01,3.825919093626539e+00}}; // 0.5 Hz BW
float Bbm[Nbm] = {-2.400978037420298e-01,7.211920560187276e-01,7.221290798214208e-01,-2.410349468880585e+00};
float Cbm[Nbm] = {0.0,0.0,0.0,-9.999999999999999e-01};
float Dbm[1] = {0.0};
float xk_bm[Nbm] = {0.0,0.0,0.0,0.0};
#define Npre 2
float Apre[Npre][Npre] = {{-1.110223024625157e-16,9.906194960589274e-01},
{-9.999999999999998e-01,1.990609672826250e+00}}; // 0.5 Hz corner
float Bpre[Npre] = {-4.911616338401903e-06,4.911616338401902e-06};
float Cpre[Npre] = {0.0,1.0};
float Dpre[1] = {0.0};
float xk_pre[Npre] = {0.0,0.0};
EndOfInitialised_CPU1_Variables
void setup1() {
// put your setup code for core 1 here, to run once:
pinMode(CORE1HB, OUTPUT);
digitalWrite(CORE1HB,HIGH); // start out high
pinMode(core1_timing_pin,OUTPUT);
digitalWrite(core1_timing_pin,HIGH);
pinMode(SBBTC, OUTPUT);
pinMode(EXTINT, OUTPUT);
pinMode(RC, OUTPUT);
pinMode(BUSY, INPUT);
pinMode(PWRD, OUTPUT);
pinMode(A2D_CS, OUTPUT);
pinMode(D2A1_CS, OUTPUT);
pinMode(D2A2_CS, OUTPUT);
pinMode(UFDC_CS_right, OUTPUT);
pinMode(UFDC_CS_left, OUTPUT);
digitalWrite(SBBTC,HIGH); // HIGH is straight binary, LOW is twos complement
digitalWrite(EXTINT,HIGH); // external clock
digitalWrite(RC,HIGH); // start out high
digitalWrite(PWRD,LOW); // powered up
digitalWrite(A2D_CS,HIGH); // start out high
digitalWrite(D2A1_CS,HIGH); // start out high
digitalWrite(D2A2_CS,HIGH); // start out high
digitalWrite(UFDC_CS_right,HIGH); // start out high
digitalWrite(UFDC_CS_left,HIGH); // start out high
analogReadResolution(12);
SPI.begin();
send_D2A_voltages(&zero,&zero);
}
void loop1() {
// put your main code for core 1 here, to run repeatedly:
if (ISRMetroCore1.check() == 1)
{
digitalWrite(core1_timing_pin,LOW);
// Measure water temperature
twelve_bit_A2D0_val = analogRead(analogPin0); // read the input pin
twelve_bit_A2D1_val = analogRead(analogPin1); // read the input pin
water_temperature_right = convert_thermistor_voltage2temp(twelve_bit_A2D0_val);
water_temperature_left = convert_thermistor_voltage2temp(twelve_bit_A2D1_val);
water_temperature_average = (water_temperature_right + water_temperature_left)/2.0;
// Measure beam angle
hallpot_voltage = read_A2D_voltage();
theta = convert_hallpot_voltage2angle(hallpot_voltage,m_v2rad,b_rad); // radians
// Estimate beam angular rate
theta_dot_est = estimate_angular_rate(theta); // radians per second
// Measure water height
read_UFDC(&UFDC_sig_right,1); // 1 for right, 2 for left
read_UFDC(&UFDC_sig_left,2); // 1 for right, 2 for left
convert_tank_periods2height(&water_height_right,&water_height_left,UFDC_sig_right,UFDC_sig_left,theta,m_p2m_right,b_m_right,m_rad2p_right,m_p2m_left,b_m_left,m_rad2p_left);
delta_h = water_height_right - water_height_left;
if (!BEAM_CONTROL_STATE_shr && !PUMP_CONTROL_STATE_shr && RESET_STATE_shr)
{
do_reset_state();
}
if (BEAM_CONTROL_STATE_shr && PUMP_CONTROL_STATE_shr)
{
do_beam_control(&delta_h_ref_bc,theta_ref_shr,theta,theta_dot_est);
}
else
{
delta_h_ref_bc = 0.0;
}
delta_h_ref_total = delta_h_ref_bc + delta_h_ref_pc_shr;
if (PUMP_CONTROL_STATE_shr)
{
do_pump_control(&pump_control_voltage_right,&pump_control_voltage_left,delta_h_ref_total,delta_h,kp_shr);
}
else
{
pump_control_voltage_right = 0.0;
pump_control_voltage_left = 0.0;
}
// Send pump input
input_Vr = pump_control_voltage_right + PumpIdleVoltageRight_shr;
input_Vl = pump_control_voltage_left + PumpIdleVoltageLeft_shr;
send_D2A_voltages(&input_Vr,&input_Vl);
// Transfer data to shared LMU memory
water_temperature_average_shr = water_temperature_average;
hallpot_voltage_shr = hallpot_voltage;
theta_shr = theta;
theta_dot_est_shr = theta_dot_est;
UFDC_sig_right_shr = UFDC_sig_right;
UFDC_sig_left_shr = UFDC_sig_left;
water_height_right_shr = water_height_right;
water_height_left_shr = water_height_left;
delta_h_shr = delta_h;
input_Vr_shr = input_Vr;
input_Vl_shr = input_Vl;
CORE1HB_flag = !CORE1HB_flag;
digitalWrite(CORE1HB,CORE1HB_flag);
digitalWrite(core1_timing_pin,HIGH);
}
}
/*** Core 2 ***/
/* CPU2 Uninitialised Data */
StartOfUninitialised_CPU2_Variables
/* Put your CPU2 fast access variables that have no initial values here e.g. uint32 CPU2_var; */
EndOfUninitialised_CPU2_Variables
/* CPU2 Initialised Data */
StartOfInitialised_CPU2_Variables
/* Put your CPU2 fast access variables that have an initial value here e.g. uint32 CPU2_var_init = 1; */
const int CORE2HB = 26;
int CORE2HB_flag = 0;
EndOfInitialised_CPU2_Variables
void setup2() {
// put your setup code for core 2 here, to run once:
pinMode(CORE2HB, OUTPUT);
digitalWrite(CORE2HB,HIGH); // start out high
}
void loop2() {
// put your main code for core 2 here, to run repeatedly:
if (ISRMetroCore2.check() == 1)
{
CORE2HB_flag = !CORE2HB_flag;
digitalWrite(CORE2HB,CORE2HB_flag);
}
}
/*** Functions ***/
float estimate_angular_rate(float th)
{
int i, j;
float th_dot;
float xkp1_re[Nre] = {0.0,0.0};
for (i = 0; i < Nre; i++)
{
xkp1_re = Bre*th;
for (j = 0; j < Nre; j++)
{
xkp1_re = xkp1_re + Are[j]*xk_re[j];
}
}
th_dot = Dre[0]*th;
for (i = 0; i < Nre; i++)
{
th_dot = th_dot + Cre*xk_re;
}
for (i = 0; i < Nre; i++)
{
xk_re = xkp1_re;
}
return th_dot;
}
void MessageLogger_CSVData(long time, float data1, float data2, float data3, float data4, float data5, float data6, float data7, float data8, float data9, float data10, float data11, String channelName)
{
Serial.print("{MESSAGE:");
Serial.print(channelName);
Serial.print("|data|");
Serial.print(time);
Serial.print(",");
Serial.print(data1,2); // 2 is number of digits to right of decimal, temperature
Serial.print(",");
Serial.print(data2,5); // 3, hallpot voltage
Serial.print(",");
Serial.print(data3,5); // 3, theta (rad.)
Serial.print(",");
Serial.print(data4,3); // 3, theta_dot
Serial.print(",");
Serial.print(data5,3); // 3, UFDC_right
Serial.print(",");
Serial.print(data6,3); // 3, UFDC_left
Serial.print(",");
Serial.print(data7,5); // 3, water_height_right
Serial.print(",");
Serial.print(data8,5); // 3, water_height_left
Serial.print(",");
Serial.print(data9,5); // 3, delta_h
Serial.print(",");
Serial.print(data10,3); // 3, input voltage right
Serial.print(",");
Serial.print(data11,3); // 3, input voltage left
Serial.println("}");
}
void Cmd_Unknown()
{
Serial.println(F("I don't understand"));
}
void Cmd_WHCCheckBoxEvent(CommandParameter &Parameters)
{
PUMP_CONTROL_STATE = Parameters.NextParameterAsInteger();
}
void Cmd_BACCheckBoxEvent(CommandParameter &Parameters)
{
BEAM_CONTROL_STATE = Parameters.NextParameterAsInteger();
}
void Cmd_ZeroStates(CommandParameter &Parameter)
{
RESET_STATE = true;
Serial.println("Feedback Prefilter and Rate Filter States Zeroed!!!!!!");
}
void do_reset_state()
{
int i;
for (i = 0; i < Nre; i++)
{
xk_re = 0.0;
}
for (i = 0; i < Nbm; i++)
{
xk_bm = 0.0;
}
for (i = 0; i < Npre; i++)
{
xk_pre = 0.0;
}
}
void do_beam_control(float *ref_dhbc, float th_ref, float th, float th_dot)
{
float err_th; //, err_th_dot, th_dot_ref;
float th_ref_pre;
// float xkp1_bm[Nbm] = {0.0,0.0,0.0,0.0,0.0,0.0,0.0};
float xkp1_bm[Nbm] = {0.0,0.0,0.0,0.0};
float xkp1_pre[Npre] = {0.0,0.0};
int i, j;
// Prefilter
for (i = 0; i < Npre; i++)
{
xkp1_pre = Bpre*th_ref*(M_PI/180.0);
for (j = 0; j < Npre; j++)
{
xkp1_pre = xkp1_pre + Apre[j]*xk_pre[j];
}
}
th_ref_pre = Dpre[0]*th_ref*(M_PI/180.0);
for (i = 0; i < Npre; i++)
{
th_ref_pre = th_ref_pre + Cpre*xk_pre;
}
for (i = 0; i < Npre; i++)
{
xk_pre = xkp1_pre;
}
// Position control
err_th = 1.0*(th_ref_pre - th);
for (i = 0; i < Nbm; i++)
{
xkp1_bm = Bbm*err_th;
for (j = 0; j < Nbm; j++)
{
xkp1_bm = xkp1_bm + Abm[j]*xk_bm[j];
}
}
*ref_dhbc = Dbm[0]*err_th;
for (i = 0; i < Nbm; i++)
{
*ref_dhbc = *ref_dhbc + Cbm*xk_bm;
}
for (i = 0; i < Nbm; i++)
{
xk_bm = xkp1_bm;
}
}
void do_pump_control(float *Vr, float *Vl, float ref_H, float delta_H, float pgain)
{
float error, u;
error = ref_H - delta_H;
u = pgain*(error);
if (u >= 0.0)
{
*Vr = u;
*Vl = 0.0;
}
else
{
*Vr = 0.0;
*Vl = -1.0*u;
}
}
float convert_thermistor_voltage2temp(int A2D_val)
{
float A2D_voltage = 0.0;
float thermistor_resistance = 0.0;
float thermistor_temperature = 0.0;
A2D_voltage = (MAX_V/MAX_A2D)*(A2D_val);
if(A2D_voltage <= (MAX_V/MAX_A2D)*(1.0))
{
A2D_voltage = (MAX_V/MAX_A2D)*(1.0);
}
thermistor_resistance = divider_resistance*((MAX_V/A2D_voltage) - 1);
if(thermistor_resistance <= MIN_RESISTANCE)
{
thermistor_resistance = MIN_RESISTANCE;
}
thermistor_temperature = THETA[0] + THETA[1]*log(thermistor_resistance) + THETA[2]*(pow(log(thermistor_resistance),3));
return thermistor_temperature;
}
float convert_hallpot_voltage2angle(float V_in, float m, float b)
{
float angle = 0.0;
angle = m*V_in + b; // degrees
return angle;
}
float read_A2D_voltage()
{
byte recieved_val_high_byte = 0;
byte recieved_val_low_byte = 0;
byte recieved_val_last_byte = 0;
unsigned short recieved_val = 0;
float voltage = 0.0;
int BUSY_val = 0;
SPI.beginTransaction(myA2DSettings); // SPI_MODE0 15150000
digitalWrite(A2D_CS,LOW);
delayMicroseconds(1);
digitalWrite(RC,LOW);
delayMicroseconds(1);
while(!BUSY_val){
BUSY_val = digitalRead(BUSY); /* BUSY is active when low */
}
delayMicroseconds(1);
digitalWrite(RC,HIGH);
delayMicroseconds(1);
recieved_val_high_byte = SPI.transfer(0x00);
recieved_val_low_byte = SPI.transfer(0x00);
recieved_val_last_byte = SPI.transfer(0x00);
digitalWrite(A2D_CS,HIGH);
SPI.endTransaction();
recieved_val = ( (((unsigned short) recieved_val_high_byte) << 9) | (((unsigned short) recieved_val_low_byte << 1) | ((unsigned short) recieved_val_last_byte) >> 7) );
voltage = (float) recieved_val*(MAX_A2D_voltage/MAX_word);
return voltage;
}
void read_UFDC(float *UFDC_sig, int side)
{
String inString = "";
byte recieved_value = 0;
byte fip_sig[13];
char fip_cup[27];
byte high_bit_mask = 0xF0;
byte low_bit_mask = 0x0F;
int UFDC_CS;
switch(side)
{
case 1 : // right
UFDC_CS = UFDC_CS_right;
break;
case 2: // left
UFDC_CS = UFDC_CS_left;
break;
default:
UFDC_CS = UFDC_CS_right;
}
//SPI.beginTransaction(SPISettings(UFDC_SCK_RATE, MSBFIRST, SPI_MODE3)); // 100 - 500 kHz
SPI.beginTransaction(myUFDCSettings); // 100 - 500 kHz
// Set accuracy 0x02, 09 = 0.001%, 06 = 0.01%, 03 = 0.1%
digitalWrite(UFDC_CS,LOW);
recieved_value = SPI.transfer(0x02);
recieved_value = SPI.transfer(0x06);
digitalWrite(UFDC_CS,HIGH);
delayMicroseconds(1);
// Get Signal Measurement ///////////////////////
// Set mode 0x06, TX1 0x01 TX2 0x0F
digitalWrite(UFDC_CS,LOW);
recieved_value = SPI.transfer(0x06);
recieved_value = SPI.transfer(0x01);
digitalWrite(UFDC_CS,HIGH);
delayMicroseconds(1); // didn't like removing this one
// Start measurement
digitalWrite(UFDC_CS,LOW);
recieved_value = SPI.transfer(0x09);
digitalWrite(UFDC_CS,HIGH);
delayMicroseconds(1);
// Check measurement status
// recieved_value = 0x01; // 0x01
// while(recieved_value != 0x00)
// {
// digitalWrite(UFDC_CS,LOW);
// recieved_value = SPI.transfer(0x03);
// recieved_value = SPI.transfer(0xFF);
// recieved_value = SPI.transfer(0xFF);
// digitalWrite(UFDC_CS,HIGH);
// delayMicroseconds(1);
// }
// delay(1);
delayMicroseconds(200);
// Get measurement!
digitalWrite(UFDC_CS,LOW);
recieved_value = SPI.transfer(0x07); // Get measurement, 07 is R1 (BCD), 08 is R2 (BINARY)
recieved_value = SPI.transfer(0xFF);
fip_sig[0] = SPI.transfer(0xFF);
fip_sig[1] = SPI.transfer(0xFF);
fip_sig[2] = SPI.transfer(0xFF);
fip_sig[3] = SPI.transfer(0xFF);
fip_sig[4] = SPI.transfer(0xFF);
fip_sig[5] = SPI.transfer(0xFF);
fip_sig[6] = SPI.transfer(0xFF);
fip_sig[7] = SPI.transfer(0xFF);
fip_sig[8] = SPI.transfer(0xFF);
fip_sig[9] = SPI.transfer(0xFF);
fip_sig[10] = SPI.transfer(0xFF);
fip_sig[11] = SPI.transfer(0xFF);
fip_sig[12] = SPI.transfer(0xFF);
digitalWrite(UFDC_CS,HIGH);
// delayMicroseconds(1);
SPI.endTransaction();
fip_cup[0] = (char) fip_sig[0];
fip_cup[1] = (char) (0x30 | (high_bit_mask & fip_sig[1]) >> 4); // high and low nibble ip5
fip_cup[2] = (char) (0x30 | low_bit_mask & fip_sig[1]);
fip_cup[3] = (char) (0x30 | (high_bit_mask & fip_sig[2]) >> 4);
fip_cup[4] = (char) (0x30 | low_bit_mask & fip_sig[2]);
fip_cup[5] = (char) (0x30 | (high_bit_mask & fip_sig[3]) >> 4);
fip_cup[6] = (char) (0x30 | low_bit_mask & fip_sig[3]);
fip_cup[7] = (char) (0x30 | (high_bit_mask & fip_sig[4]) >> 4);
fip_cup[8] = (char) (0x30 | low_bit_mask & fip_sig[4]);
fip_cup[9] = (char) (0x30 | (high_bit_mask & fip_sig[5]) >> 4);
fip_cup[10] = (char) (0x30 | low_bit_mask & fip_sig[5]);
fip_cup[11] = (char) (0x30 | (high_bit_mask & fip_sig[6]) >> 4);
fip_cup[12] = (char) (0x30 | low_bit_mask & fip_sig[6]);
strcpy(&fip_cup[13],".");
fip_cup[14] = (char) (0x30 | (high_bit_mask & fip_sig[7]) >> 4); // high and low nibble fp0
fip_cup[15] = (char) (0x30 | low_bit_mask & fip_sig[7]);
fip_cup[16] = (char) (0x30 | (high_bit_mask & fip_sig[8]) >> 4);
fip_cup[17] = (char) (0x30 | low_bit_mask & fip_sig[8]);
fip_cup[18] = (char) (0x30 | (high_bit_mask & fip_sig[9]) >> 4);
fip_cup[19] = (char) (0x30 | low_bit_mask & fip_sig[9]);
fip_cup[20] = (char) (0x30 | (high_bit_mask & fip_sig[10]) >> 4);
fip_cup[21] = (char) (0x30 | low_bit_mask & fip_sig[10]);
fip_cup[22] = (char) (0x30 | (high_bit_mask & fip_sig[11]) >> 4);
fip_cup[23] = (char) (0x30 | low_bit_mask & fip_sig[11]);
fip_cup[24] = (char) (0x30 | (high_bit_mask & fip_sig[12]) >> 4);
fip_cup[25] = (char) (0x30 | low_bit_mask & fip_sig[12]);
strcpy(&fip_cup[26],"\0");
inString = String(fip_cup);
*UFDC_sig = inString.toFloat();
// *UFDC_sig = 1.0;
//inString = "";
// delayMicroseconds(1000);
}
void convert_tank_periods2height(float *whr, float *whl, float periodr, float periodl, float angle, float mr, float br, float mar, float ml, float bl, float mal)
{
*whr = mr*(periodr - mar*angle) + br; // meters (-)
*whl = ml*(periodl - mal*angle) + bl; // meters (-)
}
void send_D2A_voltages(float *V_D2A1, float *V_D2A2)
{
unsigned short data1 = 0, data2 = 0; /* For Due short is 16 bits, int is 32 bits */
byte data1_highbyte, data1_lowbyte;
byte data2_highbyte, data2_lowbyte;
if (*V_D2A1 < MIN_D2A_voltage)
{
*V_D2A1 = MIN_D2A_voltage;
}
if (*V_D2A1 > MAX_D2A_voltage)
{
*V_D2A1 = MAX_D2A_voltage;
}
if (*V_D2A2 < MIN_D2A_voltage)
{
*V_D2A2 = MIN_D2A_voltage;
}
if (*V_D2A2 > MAX_D2A_voltage)
{
*V_D2A2 = MAX_D2A_voltage;
}
data1 = (unsigned short) (MAX_word/MAX_D2A_voltage)*(*V_D2A1);
data2 = (unsigned short) (MAX_word/MAX_D2A_voltage)*(*V_D2A2);
data1_highbyte = (byte) ((data1 & 0xFF00) >> 8);
data1_lowbyte = (byte) (data1 & 0x00FF);
data2_highbyte = (byte) ((data2 & 0xFF00) >> 8);
data2_lowbyte = (byte) (data2 & 0x00FF);
//SPI.beginTransaction(SPISettings(D2A_SCK_RATE, MSBFIRST, SPI_MODE0));
SPI.beginTransaction(myD2ASettings);
digitalWrite(D2A1_CS,LOW);
SPI.transfer(data1_highbyte);
SPI.transfer(data1_lowbyte);
digitalWrite(D2A1_CS,HIGH);
SPI.endTransaction();
//SPI.beginTransaction(SPISettings(D2A_SCK_RATE, MSBFIRST, SPI_MODE0));
SPI.beginTransaction(myD2ASettings);
digitalWrite(D2A2_CS,LOW);
SPI.transfer(data2_highbyte);
SPI.transfer(data2_lowbyte);
digitalWrite(D2A2_CS,HIGH);
SPI.endTransaction();
}
I am having trouble recompiling a sketch after having updated the Aurix Free Toolchain, Arduino IDE and ShieldBuddy IDE along with using a new license. Specifically I am using the string.h library to convert ASCII characters to a floating point number. The entire sketch is provided below but the relevant code is just:
#include <string.h>
.
.
.
inString = String(fip_cup);
*UFDC_sig = inString.toFloat();
Compile error is:
sketch\ShieldBuddyBalanceBeamSketch_v2.ino.cpp.o: In function `read_UFDC(float*, int)':
ShieldBuddyBalanceBeamSketch_v2:812: error: undefined reference to `String::operator=(String&&)'
collect2.exe: error: ld returned 1 exit status
exit status 1
undefined reference to `String::operator=(String&&)'
This code has compiled before without errors but the recent updates may have done some damage. Pls help ;-) Also I should mention that this linker/compile error does not happen when I compile for the Arduino Due target which is strange.
-Joel
/*** Don't worry, the normal Arduino setup() and loop() are below this block! ***/
#include "MegunoLink.h"
#include "CommandHandler.h"
#include <math.h>
#include <SPI.h>
//#include <SpiIf.h>
#include <Metro.h>
#include <stdio.h>
#include <string.h>
#define Serial SerialASC
#define M_PI 3.1415
Metro ISRMetroCore0 = Metro(200); // 100 ms
Metro ISRMetroCore1 = Metro(5); // 5 ms
Metro ISRMetroCore2 = Metro(1000); // 1000 ms
const unsigned int D2A_SCK_RATE = 500000; /* 10MHz, 10000000 */
const unsigned int UFDC_SCK_RATE = 500000; // 500000 100000 /* 100 - 500 kHz */
const unsigned int A2D_SCK_RATE = 500000; /* 500000 100000 1000000 5MHz */
SPISettings myD2ASettings(D2A_SCK_RATE, MSBFIRST, SPI_MODE0);
SPISettings myUFDCSettings(UFDC_SCK_RATE, MSBFIRST, SPI_MODE3); // SPI_MODE3
SPISettings myA2DSettings(A2D_SCK_RATE, MSBFIRST, SPI_MODE1); // SPI_MODE0 SPI_MODE1 for ShieldBuddy
float convert_thermistor_voltage2temp(int);
void read_UFDC(float *, int);
void convert_tank_periods2height(float *, float *, float, float, float, float, float, float, float, float, float);
void send_D2A_voltages(float *, float *);
float convert_hallpot_voltage2angle(float, float, float);
float read_A2D_voltage();
void MessageLogger_CSVData(long, float, float, float, float, float, float, float, float, float, float, float, String);
float estimate_angular_rate(float);
void do_pump_control(float *, float *, float, float, float);
void do_beam_control(float *, float, float, float);
void do_reset_state();
CommandHandler<> SerialCommandHandler(SerialASC);
TimePlot AnglePlot("Beam Angle",SerialASC); // "Channel name", appears on plot tab and drop down menu
TimePlot AngularRatePlot("Beam Angular Rate",SerialASC);
TimePlot WaterLevelPlot("Water Levels",SerialASC);
TimePlot TemperaturePlot("Water Temperature",SerialASC);
TimePlot ControlInputPlot("Control Voltages",SerialASC);
/* LMU uninitialised data */
StartOfUninitialised_LMURam_Variables
/* Put your LMU RAM fast access variables that have no initial values here e.g. uint32 LMU_var; */
EndOfUninitialised_LMURam_Variables
/* LMU uninitialised data */
StartOfInitialised_LMURam_Variables
/* Put your LMU RAM fast access variables that have an initial value here e.g. uint32 LMU_var_init = 1; */
float water_temperature_average_shr = 0.0;
float hallpot_voltage_shr = 0.0;
float theta_shr = 0.0;
float theta_dot_est_shr = 0.0;
float UFDC_sig_right_shr = 0.0;
float UFDC_sig_left_shr = 0.0;
float water_height_right_shr = 0.0;
float water_height_left_shr = 0.0;
float delta_h_shr = 0.0;
float input_Vr_shr = 0.0;
float input_Vl_shr = 0.0;
float PumpIdleVoltageRight_shr = 0.0;
float PumpIdleVoltageLeft_shr = 0.0;
boolean PUMP_CONTROL_STATE_shr = false;
boolean BEAM_CONTROL_STATE_shr = false;
boolean RESET_STATE_shr = false;
float delta_h_ref_pc_shr = 0.0;
float theta_ref_shr = 0.0;
float kp_shr = 50;
EndOfInitialised_LMURam_Variables
/* If you do not care where variables end up, declare them here! */
/*** Core 0 ***/
const int CORE0HB = 2;
int CORE0HB_flag = 0;
const int core0_timing_pin = 51;
unsigned long timer;
unsigned long timer_setup;
float PumpIdleVoltageRight = 0.0;
float PumpIdleVoltageLeft = 0.0;
boolean PUMP_CONTROL_STATE = false;
boolean BEAM_CONTROL_STATE = false;
boolean RESET_STATE = false;
float delta_h_ref_pc = 0.0;
float theta_ref = 0.0;
float kp = 50.0;
void setup() {
// put your setup code for core 0 here, to run once:
pinMode(CORE0HB, OUTPUT);
digitalWrite(CORE0HB,HIGH); // start out high
pinMode(core0_timing_pin,OUTPUT);
digitalWrite(core0_timing_pin,HIGH);
Serial.begin(115200);
Serial.println("\n");
Serial.println("\n");
Serial.println("\n");
Serial.println("*************************************************************");
Serial.println("********** Starting Three Core Balance Beam Sketch **********");
Serial.println("*************************************************************");
Serial.println("\n");
Serial.println("Serial Connection Opened At 115200 Baud Rate On Core 0 ...");
Serial.println("\n");
AnglePlot.SetTitle("Hallpot Angle Measurement");
AnglePlot.SetXlabel("Time (sec.)");
AnglePlot.SetYlabel("Angle (deg.)");
AnglePlot.SetSeriesProperties("Calibrated Hallpot Angle", TimePlot::Red, TimePlot::Solid, 3, TimePlot::Plus); // legend tag
AngularRatePlot.SetTitle("Estimated Angular Rate");
AngularRatePlot.SetXlabel("Time (sec.)");
AngularRatePlot.SetYlabel("Angular Rate (deg./sec.)");
AngularRatePlot.SetSeriesProperties("Estimated Angular Rate", TimePlot::Red, TimePlot::Solid, 3, TimePlot::Plus);
WaterLevelPlot.SetTitle("Water Level Measurements");
WaterLevelPlot.SetXlabel("Time (sec.)");
WaterLevelPlot.SetYlabel("Height (m)");
WaterLevelPlot.SetSeriesProperties("Calibrated Water Level (Right - Left)", TimePlot::Blue, TimePlot::Solid, 3, TimePlot::Plus);
TemperaturePlot.SetTitle("Temperature Measurements");
TemperaturePlot.SetXlabel("Time (sec.)");
TemperaturePlot.SetYlabel("Temperature (deg. C)");
TemperaturePlot.SetSeriesProperties("Calibrated Water Temperature (Average)", TimePlot::Green, TimePlot::Solid, 3, TimePlot::Plus);
ControlInputPlot.SetTitle("Control Input Voltages");
ControlInputPlot.SetXlabel("Time (sec.)");
ControlInputPlot.SetYlabel("Voltage (V)");
ControlInputPlot.SetSeriesProperties("Right Pump Voltage", TimePlot::Magenta, TimePlot::Solid, 3, TimePlot::Plus);
ControlInputPlot.SetSeriesProperties("Left Pump Voltage", TimePlot::Cyan, TimePlot::Solid, 3, TimePlot::Plus);
SerialCommandHandler.AddVariable(F("PumpIdleVoltageRight"), PumpIdleVoltageRight);
SerialCommandHandler.AddVariable(F("PumpIdleVoltageLeft"), PumpIdleVoltageLeft);
SerialCommandHandler.SetDefaultHandler(Cmd_Unknown);
SerialCommandHandler.AddCommand(F("WHCCheckBoxEvent"), Cmd_WHCCheckBoxEvent);
SerialCommandHandler.AddCommand(F("BACCheckBoxEvent"), Cmd_BACCheckBoxEvent);
SerialCommandHandler.AddCommand(F("ZeroFBCStates"), Cmd_ZeroStates);
SerialCommandHandler.AddVariable(F("ReferenceWaterHeight"), delta_h_ref_pc);
SerialCommandHandler.AddVariable(F("ReferenceBeamAngle"), theta_ref);
SerialCommandHandler.AddVariable(F("Kp"), kp);
timer_setup = millis();
}
void loop() {
SerialCommandHandler.Process();
// put your main code for core 0 here, to run repeatedly:
if (ISRMetroCore0.check() == 1)
{
digitalWrite(core0_timing_pin,LOW);
AnglePlot.SendFloatData("Calibrated Hallpot Angle", theta_shr*(180.0/M_PI), 5); // "Series name"
AngularRatePlot.SendFloatData("Estimated Angular Rate", theta_dot_est_shr*(180.0/M_PI), 4);
WaterLevelPlot.SendFloatData("Calibrated Water Level (Right - Left)", delta_h_shr, 4); // delta_h
TemperaturePlot.SendFloatData("Calibrated Water Temperature (Average)", water_temperature_average_shr, 4); // water_temperature_average
ControlInputPlot.SendFloatData("Right Pump Voltage", input_Vr_shr, 4);
ControlInputPlot.SendFloatData("Left Pump Voltage", input_Vl_shr, 4);
timer = millis() - timer_setup;
MessageLogger_CSVData(timer, water_temperature_average_shr, hallpot_voltage_shr, theta_shr, theta_dot_est_shr, UFDC_sig_right_shr, UFDC_sig_left_shr, water_height_right_shr, water_height_left_shr, delta_h_shr, input_Vr_shr, input_Vl_shr, "Sensor_Actuator_Data");
// Transfer data to shared LMU memory
PumpIdleVoltageRight_shr = PumpIdleVoltageRight;
PumpIdleVoltageLeft_shr = PumpIdleVoltageLeft;
PUMP_CONTROL_STATE_shr = PUMP_CONTROL_STATE;
BEAM_CONTROL_STATE_shr = BEAM_CONTROL_STATE;
RESET_STATE_shr = RESET_STATE;
delta_h_ref_pc_shr = delta_h_ref_pc;
theta_ref_shr = theta_ref;
kp_shr = kp;
RESET_STATE = false;
CORE0HB_flag = !CORE0HB_flag;
digitalWrite(CORE0HB,CORE0HB_flag);
digitalWrite(core0_timing_pin,HIGH);
}
}
/*** Core 1 ***/
/* CPU1 Uninitialised Data */
StartOfUninitialised_CPU1_Variables
/* Put your CPU1 fast access variables that have no initial values here e.g. uint32 CPU1_var; */
EndOfUninitialised_CPU1_Variables
/* CPU1 Initialised Data */
StartOfInitialised_CPU1_Variables
/* Put your CPU1 fast access variables that have an initial value here e.g. uint32 CPU1_var_init = 1; */
const int CORE1HB = 22;
int CORE1HB_flag = 0;
const int core1_timing_pin = 53;
// Thermistor parameters
float MAX_V = 5.0; // 3.3 Reference voltage for A2D channels with Due. 5.0 with ShieldBuddy
int MAX_A2D = 1023;
int analogPin0 = 0;
int analogPin1 = 1;
float THETA[3] = {3.33746911687670e+2,-3.83716062783759e+1,5.73463936130444e-2};
float MIN_RESISTANCE = 303.075;
float MAX_RESISTANCE = 367701.4;
float divider_resistance = 10000; // Ohms
// Thermistor variables
int twelve_bit_A2D0_val = 0;
int twelve_bit_A2D1_val = 0;
float water_temperature_right = 0.0;
float water_temperature_left = 0.0;
float water_temperature_average = 0.0;
// Hallpot parameters
float m_v2rad = 4.439744921303501e-01; // 4.507506390723722e-01; 25.2643013874206; -16.7918701445676;
float b_rad = -1.106880081759474; // -1.110281348181693; -59.4501610744572; 39.1664878889829;
const int SBBTC = 4; // SBBTC, straight binary or binary twos complement, output (HIGH for SB on pin 8 of AD977)
const int EXTINT = 5; // EXTINT external or internal clock, output (HIGH for external on pin 9 of AD977)
const int RC = 6; // RC, read/convert, output
const int BUSY = 3; // BUSY, input
const int PWRD = 7; // PWRD, power down, output (LOW for powered up on pin 18 of AD977)
const int A2D_CS = 11; // A2D CS, output
const unsigned short MAX_word = 65535; /* 2^16 - 1 = 65535 is maximum unsigned short */
const float MAX_A2D_voltage = 5.0;
// Hallpot variables
float hallpot_voltage = 0.0;
float theta = 0.0;
float theta_dot_est = 0.0;
// Water tank parameters
float m_p2m_right = 5.912248633713175e-04;
float b_m_right = -1.279781180930623e-01;
float m_rad2p_right = 4.107996159153647;
float m_p2m_left = 5.642928988291027e-04;
float b_m_left = -1.267508779208012e-01;
float m_rad2p_left = -2.931498670086657;
const int UFDC_CS_right = 12; // 12 UFDC CS output (right tank)
const int UFDC_CS_left = 8; // 13 UFDC CS output (left tank)
// Water tank variables
float UFDC_sig_right = 100.0;
float UFDC_sig_left = 200.0;
float water_height_right = 0.0;
float water_height_left = 0.0;
float delta_h = 0.0; // right - left
// D2A parameters
const int D2A1_CS = 9; /* D2A chip selects */
const int D2A2_CS = 10;
const float MAX_D2A_voltage = 5.0;
const float MIN_D2A_voltage = 0.0;
// D2A variables
float input_Vr = 0.0;
float input_Vl = 0.0;
float zero = 0.0;
// Rate estimator filter parameters
#define Nre 2
//float Are[Nre][Nre] = {{-1.110223024625157e-16,9.906194960589272e-01},{-1.000000000000000e+00,1.988409675872070e+00}}; // 1.5 Hz corner
//float Bre[Nre] = {-2.513258523646844,-4.648541838448669e-01};
//float Cre[Nre] = {0.0,-1.0};
//float Dre[1] = {0.0};
float Are[Nre][Nre] = {{-2.220446049250313e-16,9.782488939094772e-01},{-1.000000000000000e+00,1.966302642061996e+00}}; // 3.5 Hz corner
float Bre[Nre] = {-2.513258523646844,-2.513258523646843};
float Cre[Nre] = {0.0,-1.0};
float Dre[1] = {0.0};
float xk_re[Nre] = {0.0,0.0};
// Pump control variables
float delta_h_ref_total = 0.0;
float delta_h_ref_bc = 0.0;
float pump_control_voltage_right = 0.0;
float pump_control_voltage_left = 0.0;
// Beam control filters
//#define Nbm 7
//float Abm[Nbm][Nbm] = {{-6.453171330633722e-16,-3.592757680011890e-16,1.737283960164751e-16,1.779994139821527e-15,4.253745440190021e-16,-8.448260360106643e-15,-1.970287298659054e-05},
// {1.000000000000002e-01,-4.600194883798486e-16,-1.263565400178538e-16,-8.268511417045545e-16,-1.976873611380493e-15,-1.010096454786383e-16,2.311104748464772e-05},
// {0.000000000000000e+00,1.000000000000001e+00,6.330127011466199e-16,1.134069082291906e-15,3.170283497285367e-16,1.051300413025089e-16,-1.110555110601209e-04},
// {0.000000000000000e+00,0.000000000000000e+00,-9.999999999999994e-01,6.153468078650685e-16,2.377423854111574e-16,6.441672352387496e-17,-2.952827105707959e-04},
// {0.000000000000000e+00,0.000000000000000e+00,0.000000000000000e+00,9.999999999999998e+00,-5.544184936907761e-15,-8.553179697172885e-16,4.931606683667007e-03},
// {0.000000000000000e+00,0.000000000000000e+00,0.000000000000000e+00,0.000000000000000e+00,1.000000000000000e+01,1.848798365247152e-15,-5.253664833940819e-02},
// {0.000000000000000e+00,0.000000000000000e+00,0.000000000000000e+00,0.000000000000000e+00,0.000000000000000e+00,1.000000000000000e+02,3.375737747240404e+00}}; // 10.0 Hz BW
//float Bbm[Nbm] = {-4.772120388909852e-01,2.960723091657326e-01,-7.835695530322973e-01,-1.128468652061411e+00,9.288999636979765e+00,-4.126379379248117e+01,7.698817900722686e+02};
//float Cbm[Nbm] = {0.0,0.0,0.0,0.0,0.0,0.0,1000.0};
//float Dbm[1] = {0.0};
//float xk_bm[Nbm] = {0.0,0.0,0.0,0.0,0.0,0.0,0.0};
#define Nbm 4
float Abm[Nbm][Nbm] = {{-4.440892098500626e-16,9.109600434118143e-17,-6.670338036882551e-16,8.334243240386791e-02},
{1.000000000000000e+00,2.024977149732429e-16,3.642692133582339e-16,-3.493334277882195e-01},
{0.000000000000000e+00,-1.000000000000000e+00,-5.915248990139340e-16,-5.485829047470054e-01},
{0.000000000000000e+00,0.000000000000000e+00,1.000000000000000e+01,3.825919093626539e+00}}; // 0.5 Hz BW
float Bbm[Nbm] = {-2.400978037420298e-01,7.211920560187276e-01,7.221290798214208e-01,-2.410349468880585e+00};
float Cbm[Nbm] = {0.0,0.0,0.0,-9.999999999999999e-01};
float Dbm[1] = {0.0};
float xk_bm[Nbm] = {0.0,0.0,0.0,0.0};
#define Npre 2
float Apre[Npre][Npre] = {{-1.110223024625157e-16,9.906194960589274e-01},
{-9.999999999999998e-01,1.990609672826250e+00}}; // 0.5 Hz corner
float Bpre[Npre] = {-4.911616338401903e-06,4.911616338401902e-06};
float Cpre[Npre] = {0.0,1.0};
float Dpre[1] = {0.0};
float xk_pre[Npre] = {0.0,0.0};
EndOfInitialised_CPU1_Variables
void setup1() {
// put your setup code for core 1 here, to run once:
pinMode(CORE1HB, OUTPUT);
digitalWrite(CORE1HB,HIGH); // start out high
pinMode(core1_timing_pin,OUTPUT);
digitalWrite(core1_timing_pin,HIGH);
pinMode(SBBTC, OUTPUT);
pinMode(EXTINT, OUTPUT);
pinMode(RC, OUTPUT);
pinMode(BUSY, INPUT);
pinMode(PWRD, OUTPUT);
pinMode(A2D_CS, OUTPUT);
pinMode(D2A1_CS, OUTPUT);
pinMode(D2A2_CS, OUTPUT);
pinMode(UFDC_CS_right, OUTPUT);
pinMode(UFDC_CS_left, OUTPUT);
digitalWrite(SBBTC,HIGH); // HIGH is straight binary, LOW is twos complement
digitalWrite(EXTINT,HIGH); // external clock
digitalWrite(RC,HIGH); // start out high
digitalWrite(PWRD,LOW); // powered up
digitalWrite(A2D_CS,HIGH); // start out high
digitalWrite(D2A1_CS,HIGH); // start out high
digitalWrite(D2A2_CS,HIGH); // start out high
digitalWrite(UFDC_CS_right,HIGH); // start out high
digitalWrite(UFDC_CS_left,HIGH); // start out high
analogReadResolution(12);
SPI.begin();
send_D2A_voltages(&zero,&zero);
}
void loop1() {
// put your main code for core 1 here, to run repeatedly:
if (ISRMetroCore1.check() == 1)
{
digitalWrite(core1_timing_pin,LOW);
// Measure water temperature
twelve_bit_A2D0_val = analogRead(analogPin0); // read the input pin
twelve_bit_A2D1_val = analogRead(analogPin1); // read the input pin
water_temperature_right = convert_thermistor_voltage2temp(twelve_bit_A2D0_val);
water_temperature_left = convert_thermistor_voltage2temp(twelve_bit_A2D1_val);
water_temperature_average = (water_temperature_right + water_temperature_left)/2.0;
// Measure beam angle
hallpot_voltage = read_A2D_voltage();
theta = convert_hallpot_voltage2angle(hallpot_voltage,m_v2rad,b_rad); // radians
// Estimate beam angular rate
theta_dot_est = estimate_angular_rate(theta); // radians per second
// Measure water height
read_UFDC(&UFDC_sig_right,1); // 1 for right, 2 for left
read_UFDC(&UFDC_sig_left,2); // 1 for right, 2 for left
convert_tank_periods2height(&water_height_right,&water_height_left,UFDC_sig_right,UFDC_sig_left,theta,m_p2m_right,b_m_right,m_rad2p_right,m_p2m_left,b_m_left,m_rad2p_left);
delta_h = water_height_right - water_height_left;
if (!BEAM_CONTROL_STATE_shr && !PUMP_CONTROL_STATE_shr && RESET_STATE_shr)
{
do_reset_state();
}
if (BEAM_CONTROL_STATE_shr && PUMP_CONTROL_STATE_shr)
{
do_beam_control(&delta_h_ref_bc,theta_ref_shr,theta,theta_dot_est);
}
else
{
delta_h_ref_bc = 0.0;
}
delta_h_ref_total = delta_h_ref_bc + delta_h_ref_pc_shr;
if (PUMP_CONTROL_STATE_shr)
{
do_pump_control(&pump_control_voltage_right,&pump_control_voltage_left,delta_h_ref_total,delta_h,kp_shr);
}
else
{
pump_control_voltage_right = 0.0;
pump_control_voltage_left = 0.0;
}
// Send pump input
input_Vr = pump_control_voltage_right + PumpIdleVoltageRight_shr;
input_Vl = pump_control_voltage_left + PumpIdleVoltageLeft_shr;
send_D2A_voltages(&input_Vr,&input_Vl);
// Transfer data to shared LMU memory
water_temperature_average_shr = water_temperature_average;
hallpot_voltage_shr = hallpot_voltage;
theta_shr = theta;
theta_dot_est_shr = theta_dot_est;
UFDC_sig_right_shr = UFDC_sig_right;
UFDC_sig_left_shr = UFDC_sig_left;
water_height_right_shr = water_height_right;
water_height_left_shr = water_height_left;
delta_h_shr = delta_h;
input_Vr_shr = input_Vr;
input_Vl_shr = input_Vl;
CORE1HB_flag = !CORE1HB_flag;
digitalWrite(CORE1HB,CORE1HB_flag);
digitalWrite(core1_timing_pin,HIGH);
}
}
/*** Core 2 ***/
/* CPU2 Uninitialised Data */
StartOfUninitialised_CPU2_Variables
/* Put your CPU2 fast access variables that have no initial values here e.g. uint32 CPU2_var; */
EndOfUninitialised_CPU2_Variables
/* CPU2 Initialised Data */
StartOfInitialised_CPU2_Variables
/* Put your CPU2 fast access variables that have an initial value here e.g. uint32 CPU2_var_init = 1; */
const int CORE2HB = 26;
int CORE2HB_flag = 0;
EndOfInitialised_CPU2_Variables
void setup2() {
// put your setup code for core 2 here, to run once:
pinMode(CORE2HB, OUTPUT);
digitalWrite(CORE2HB,HIGH); // start out high
}
void loop2() {
// put your main code for core 2 here, to run repeatedly:
if (ISRMetroCore2.check() == 1)
{
CORE2HB_flag = !CORE2HB_flag;
digitalWrite(CORE2HB,CORE2HB_flag);
}
}
/*** Functions ***/
float estimate_angular_rate(float th)
{
int i, j;
float th_dot;
float xkp1_re[Nre] = {0.0,0.0};
for (i = 0; i < Nre; i++)
{
xkp1_re = Bre*th;
for (j = 0; j < Nre; j++)
{
xkp1_re = xkp1_re + Are[j]*xk_re[j];
}
}
th_dot = Dre[0]*th;
for (i = 0; i < Nre; i++)
{
th_dot = th_dot + Cre*xk_re;
}
for (i = 0; i < Nre; i++)
{
xk_re = xkp1_re;
}
return th_dot;
}
void MessageLogger_CSVData(long time, float data1, float data2, float data3, float data4, float data5, float data6, float data7, float data8, float data9, float data10, float data11, String channelName)
{
Serial.print("{MESSAGE:");
Serial.print(channelName);
Serial.print("|data|");
Serial.print(time);
Serial.print(",");
Serial.print(data1,2); // 2 is number of digits to right of decimal, temperature
Serial.print(",");
Serial.print(data2,5); // 3, hallpot voltage
Serial.print(",");
Serial.print(data3,5); // 3, theta (rad.)
Serial.print(",");
Serial.print(data4,3); // 3, theta_dot
Serial.print(",");
Serial.print(data5,3); // 3, UFDC_right
Serial.print(",");
Serial.print(data6,3); // 3, UFDC_left
Serial.print(",");
Serial.print(data7,5); // 3, water_height_right
Serial.print(",");
Serial.print(data8,5); // 3, water_height_left
Serial.print(",");
Serial.print(data9,5); // 3, delta_h
Serial.print(",");
Serial.print(data10,3); // 3, input voltage right
Serial.print(",");
Serial.print(data11,3); // 3, input voltage left
Serial.println("}");
}
void Cmd_Unknown()
{
Serial.println(F("I don't understand"));
}
void Cmd_WHCCheckBoxEvent(CommandParameter &Parameters)
{
PUMP_CONTROL_STATE = Parameters.NextParameterAsInteger();
}
void Cmd_BACCheckBoxEvent(CommandParameter &Parameters)
{
BEAM_CONTROL_STATE = Parameters.NextParameterAsInteger();
}
void Cmd_ZeroStates(CommandParameter &Parameter)
{
RESET_STATE = true;
Serial.println("Feedback Prefilter and Rate Filter States Zeroed!!!!!!");
}
void do_reset_state()
{
int i;
for (i = 0; i < Nre; i++)
{
xk_re = 0.0;
}
for (i = 0; i < Nbm; i++)
{
xk_bm = 0.0;
}
for (i = 0; i < Npre; i++)
{
xk_pre = 0.0;
}
}
void do_beam_control(float *ref_dhbc, float th_ref, float th, float th_dot)
{
float err_th; //, err_th_dot, th_dot_ref;
float th_ref_pre;
// float xkp1_bm[Nbm] = {0.0,0.0,0.0,0.0,0.0,0.0,0.0};
float xkp1_bm[Nbm] = {0.0,0.0,0.0,0.0};
float xkp1_pre[Npre] = {0.0,0.0};
int i, j;
// Prefilter
for (i = 0; i < Npre; i++)
{
xkp1_pre = Bpre*th_ref*(M_PI/180.0);
for (j = 0; j < Npre; j++)
{
xkp1_pre = xkp1_pre + Apre[j]*xk_pre[j];
}
}
th_ref_pre = Dpre[0]*th_ref*(M_PI/180.0);
for (i = 0; i < Npre; i++)
{
th_ref_pre = th_ref_pre + Cpre*xk_pre;
}
for (i = 0; i < Npre; i++)
{
xk_pre = xkp1_pre;
}
// Position control
err_th = 1.0*(th_ref_pre - th);
for (i = 0; i < Nbm; i++)
{
xkp1_bm = Bbm*err_th;
for (j = 0; j < Nbm; j++)
{
xkp1_bm = xkp1_bm + Abm[j]*xk_bm[j];
}
}
*ref_dhbc = Dbm[0]*err_th;
for (i = 0; i < Nbm; i++)
{
*ref_dhbc = *ref_dhbc + Cbm*xk_bm;
}
for (i = 0; i < Nbm; i++)
{
xk_bm = xkp1_bm;
}
}
void do_pump_control(float *Vr, float *Vl, float ref_H, float delta_H, float pgain)
{
float error, u;
error = ref_H - delta_H;
u = pgain*(error);
if (u >= 0.0)
{
*Vr = u;
*Vl = 0.0;
}
else
{
*Vr = 0.0;
*Vl = -1.0*u;
}
}
float convert_thermistor_voltage2temp(int A2D_val)
{
float A2D_voltage = 0.0;
float thermistor_resistance = 0.0;
float thermistor_temperature = 0.0;
A2D_voltage = (MAX_V/MAX_A2D)*(A2D_val);
if(A2D_voltage <= (MAX_V/MAX_A2D)*(1.0))
{
A2D_voltage = (MAX_V/MAX_A2D)*(1.0);
}
thermistor_resistance = divider_resistance*((MAX_V/A2D_voltage) - 1);
if(thermistor_resistance <= MIN_RESISTANCE)
{
thermistor_resistance = MIN_RESISTANCE;
}
thermistor_temperature = THETA[0] + THETA[1]*log(thermistor_resistance) + THETA[2]*(pow(log(thermistor_resistance),3));
return thermistor_temperature;
}
float convert_hallpot_voltage2angle(float V_in, float m, float b)
{
float angle = 0.0;
angle = m*V_in + b; // degrees
return angle;
}
float read_A2D_voltage()
{
byte recieved_val_high_byte = 0;
byte recieved_val_low_byte = 0;
byte recieved_val_last_byte = 0;
unsigned short recieved_val = 0;
float voltage = 0.0;
int BUSY_val = 0;
SPI.beginTransaction(myA2DSettings); // SPI_MODE0 15150000
digitalWrite(A2D_CS,LOW);
delayMicroseconds(1);
digitalWrite(RC,LOW);
delayMicroseconds(1);
while(!BUSY_val){
BUSY_val = digitalRead(BUSY); /* BUSY is active when low */
}
delayMicroseconds(1);
digitalWrite(RC,HIGH);
delayMicroseconds(1);
recieved_val_high_byte = SPI.transfer(0x00);
recieved_val_low_byte = SPI.transfer(0x00);
recieved_val_last_byte = SPI.transfer(0x00);
digitalWrite(A2D_CS,HIGH);
SPI.endTransaction();
recieved_val = ( (((unsigned short) recieved_val_high_byte) << 9) | (((unsigned short) recieved_val_low_byte << 1) | ((unsigned short) recieved_val_last_byte) >> 7) );
voltage = (float) recieved_val*(MAX_A2D_voltage/MAX_word);
return voltage;
}
void read_UFDC(float *UFDC_sig, int side)
{
String inString = "";
byte recieved_value = 0;
byte fip_sig[13];
char fip_cup[27];
byte high_bit_mask = 0xF0;
byte low_bit_mask = 0x0F;
int UFDC_CS;
switch(side)
{
case 1 : // right
UFDC_CS = UFDC_CS_right;
break;
case 2: // left
UFDC_CS = UFDC_CS_left;
break;
default:
UFDC_CS = UFDC_CS_right;
}
//SPI.beginTransaction(SPISettings(UFDC_SCK_RATE, MSBFIRST, SPI_MODE3)); // 100 - 500 kHz
SPI.beginTransaction(myUFDCSettings); // 100 - 500 kHz
// Set accuracy 0x02, 09 = 0.001%, 06 = 0.01%, 03 = 0.1%
digitalWrite(UFDC_CS,LOW);
recieved_value = SPI.transfer(0x02);
recieved_value = SPI.transfer(0x06);
digitalWrite(UFDC_CS,HIGH);
delayMicroseconds(1);
// Get Signal Measurement ///////////////////////
// Set mode 0x06, TX1 0x01 TX2 0x0F
digitalWrite(UFDC_CS,LOW);
recieved_value = SPI.transfer(0x06);
recieved_value = SPI.transfer(0x01);
digitalWrite(UFDC_CS,HIGH);
delayMicroseconds(1); // didn't like removing this one
// Start measurement
digitalWrite(UFDC_CS,LOW);
recieved_value = SPI.transfer(0x09);
digitalWrite(UFDC_CS,HIGH);
delayMicroseconds(1);
// Check measurement status
// recieved_value = 0x01; // 0x01
// while(recieved_value != 0x00)
// {
// digitalWrite(UFDC_CS,LOW);
// recieved_value = SPI.transfer(0x03);
// recieved_value = SPI.transfer(0xFF);
// recieved_value = SPI.transfer(0xFF);
// digitalWrite(UFDC_CS,HIGH);
// delayMicroseconds(1);
// }
// delay(1);
delayMicroseconds(200);
// Get measurement!
digitalWrite(UFDC_CS,LOW);
recieved_value = SPI.transfer(0x07); // Get measurement, 07 is R1 (BCD), 08 is R2 (BINARY)
recieved_value = SPI.transfer(0xFF);
fip_sig[0] = SPI.transfer(0xFF);
fip_sig[1] = SPI.transfer(0xFF);
fip_sig[2] = SPI.transfer(0xFF);
fip_sig[3] = SPI.transfer(0xFF);
fip_sig[4] = SPI.transfer(0xFF);
fip_sig[5] = SPI.transfer(0xFF);
fip_sig[6] = SPI.transfer(0xFF);
fip_sig[7] = SPI.transfer(0xFF);
fip_sig[8] = SPI.transfer(0xFF);
fip_sig[9] = SPI.transfer(0xFF);
fip_sig[10] = SPI.transfer(0xFF);
fip_sig[11] = SPI.transfer(0xFF);
fip_sig[12] = SPI.transfer(0xFF);
digitalWrite(UFDC_CS,HIGH);
// delayMicroseconds(1);
SPI.endTransaction();
fip_cup[0] = (char) fip_sig[0];
fip_cup[1] = (char) (0x30 | (high_bit_mask & fip_sig[1]) >> 4); // high and low nibble ip5
fip_cup[2] = (char) (0x30 | low_bit_mask & fip_sig[1]);
fip_cup[3] = (char) (0x30 | (high_bit_mask & fip_sig[2]) >> 4);
fip_cup[4] = (char) (0x30 | low_bit_mask & fip_sig[2]);
fip_cup[5] = (char) (0x30 | (high_bit_mask & fip_sig[3]) >> 4);
fip_cup[6] = (char) (0x30 | low_bit_mask & fip_sig[3]);
fip_cup[7] = (char) (0x30 | (high_bit_mask & fip_sig[4]) >> 4);
fip_cup[8] = (char) (0x30 | low_bit_mask & fip_sig[4]);
fip_cup[9] = (char) (0x30 | (high_bit_mask & fip_sig[5]) >> 4);
fip_cup[10] = (char) (0x30 | low_bit_mask & fip_sig[5]);
fip_cup[11] = (char) (0x30 | (high_bit_mask & fip_sig[6]) >> 4);
fip_cup[12] = (char) (0x30 | low_bit_mask & fip_sig[6]);
strcpy(&fip_cup[13],".");
fip_cup[14] = (char) (0x30 | (high_bit_mask & fip_sig[7]) >> 4); // high and low nibble fp0
fip_cup[15] = (char) (0x30 | low_bit_mask & fip_sig[7]);
fip_cup[16] = (char) (0x30 | (high_bit_mask & fip_sig[8]) >> 4);
fip_cup[17] = (char) (0x30 | low_bit_mask & fip_sig[8]);
fip_cup[18] = (char) (0x30 | (high_bit_mask & fip_sig[9]) >> 4);
fip_cup[19] = (char) (0x30 | low_bit_mask & fip_sig[9]);
fip_cup[20] = (char) (0x30 | (high_bit_mask & fip_sig[10]) >> 4);
fip_cup[21] = (char) (0x30 | low_bit_mask & fip_sig[10]);
fip_cup[22] = (char) (0x30 | (high_bit_mask & fip_sig[11]) >> 4);
fip_cup[23] = (char) (0x30 | low_bit_mask & fip_sig[11]);
fip_cup[24] = (char) (0x30 | (high_bit_mask & fip_sig[12]) >> 4);
fip_cup[25] = (char) (0x30 | low_bit_mask & fip_sig[12]);
strcpy(&fip_cup[26],"\0");
inString = String(fip_cup);
*UFDC_sig = inString.toFloat();
// *UFDC_sig = 1.0;
//inString = "";
// delayMicroseconds(1000);
}
void convert_tank_periods2height(float *whr, float *whl, float periodr, float periodl, float angle, float mr, float br, float mar, float ml, float bl, float mal)
{
*whr = mr*(periodr - mar*angle) + br; // meters (-)
*whl = ml*(periodl - mal*angle) + bl; // meters (-)
}
void send_D2A_voltages(float *V_D2A1, float *V_D2A2)
{
unsigned short data1 = 0, data2 = 0; /* For Due short is 16 bits, int is 32 bits */
byte data1_highbyte, data1_lowbyte;
byte data2_highbyte, data2_lowbyte;
if (*V_D2A1 < MIN_D2A_voltage)
{
*V_D2A1 = MIN_D2A_voltage;
}
if (*V_D2A1 > MAX_D2A_voltage)
{
*V_D2A1 = MAX_D2A_voltage;
}
if (*V_D2A2 < MIN_D2A_voltage)
{
*V_D2A2 = MIN_D2A_voltage;
}
if (*V_D2A2 > MAX_D2A_voltage)
{
*V_D2A2 = MAX_D2A_voltage;
}
data1 = (unsigned short) (MAX_word/MAX_D2A_voltage)*(*V_D2A1);
data2 = (unsigned short) (MAX_word/MAX_D2A_voltage)*(*V_D2A2);
data1_highbyte = (byte) ((data1 & 0xFF00) >> 8);
data1_lowbyte = (byte) (data1 & 0x00FF);
data2_highbyte = (byte) ((data2 & 0xFF00) >> 8);
data2_lowbyte = (byte) (data2 & 0x00FF);
//SPI.beginTransaction(SPISettings(D2A_SCK_RATE, MSBFIRST, SPI_MODE0));
SPI.beginTransaction(myD2ASettings);
digitalWrite(D2A1_CS,LOW);
SPI.transfer(data1_highbyte);
SPI.transfer(data1_lowbyte);
digitalWrite(D2A1_CS,HIGH);
SPI.endTransaction();
//SPI.beginTransaction(SPISettings(D2A_SCK_RATE, MSBFIRST, SPI_MODE0));
SPI.beginTransaction(myD2ASettings);
digitalWrite(D2A2_CS,LOW);
SPI.transfer(data2_highbyte);
SPI.transfer(data2_lowbyte);
digitalWrite(D2A2_CS,HIGH);
SPI.endTransaction();
}