void update_energy_and_temperature () {
for (int h=0; h < num_replicas; h++) {
kinetic_energy[h] = GetKineticEnergy(h);
total_energy[h] = kinetic_energy[h] + potential_energy[h];
current_temperature[h] = (2 * kinetic_energy[h])/((3 * N) - 6);
}
}
Real DynamicChain2D::GetKineticEnergy() const
{
Real val=Zero;
for(unsigned int i=0;i<links.size();i++)
val+=GetKineticEnergy(i);
return val;
}
int main(int argc, char** argv){
// declare metal properties
float m, a, n, c, density, SetpointTemperature;
// check for second argument, and initialize metal properties accordingly
if (argc < 2) {
std::cout << "Program must be invoked with argument \'gold\' or \'silver\'" << std::endl;
return -1; // terminate with error
}
else if (strcmp(argv[1],"gold") == 0) {
m = gold_m;
a = gold_a;
n = gold_n;
c = gold_c;
density = gold_density;
SetpointTemperature = gold_temperature;
}
else if (strcmp(argv[1],"silver") == 0) {
m = silver_m;
a = silver_a;
n = silver_n;
c = silver_c;
density = silver_density;
SetpointTemperature = silver_temperature;
}
else {
std::cout << "Argument must be \'gold\' or \'silver\'\n\n";
return -1;
}
// declaring variables
int timestart = time (NULL), timend, steps = 10; // Number of time steps to take
float KineticEnergy = 0, CurrentTemperature, TotalEnergy, timeEvolved, deltaT = 0.001; // Size of time step
float b = pow(abs(N/density) , (1.0/3.0));
float rc = b/2;
float BestV = 0;
printf("Box length = %f.\n", b);
FILE * fileVelocityVerlet;
fileVelocityVerlet = fopen("Results.txt", "w");
if (fileVelocityVerlet == NULL) {
std::cout << "Unable to open VelocityVerlet.txt" << std::endl;
exit(1); // terminate with error
}
FILE * fileBestConfig;
fileBestConfig = fopen("BestConfig.txt", "w");
if (fileBestConfig == NULL) {
std::cout << "Unable to open BestConfig.txt" << std::endl;
exit(1); // terminate with error
}
// Initialize positions and forces
GetInitPositionsAndInitVelocities(SetpointTemperature);
CalcForces(b, rc, a, c, m, n);
// tau =
// Calculate initial kinetic and total energy and temperature
KineticEnergy = GetKineticEnergy();
TotalEnergy = KineticEnergy + Vfinal;
CurrentTemperature = (2 * KineticEnergy)/(3 * N - 3);
std::cout << "Starting Temperature = " << CurrentTemperature << std::endl;
fprintf (fileVelocityVerlet, "Time, Total Energy, PotentialE(V), KineticE \n", TotalEnergy, Vfinal, KineticEnergy); // Print starting values
fprintf (fileVelocityVerlet, "0, %f, %f, %f\n", TotalEnergy, Vfinal, KineticEnergy); // Print starting values
for (int i = 0; i < steps; i++){
// Update position and velocity
VelocityVerlet(deltaT, b, rc, a, c, m, n);
// Get Kinetic Energy
KineticEnergy = GetKineticEnergy();
// std::cout << Vfinal << std::endl;
for (int v = 0; v < 1; v++) {
std::cout << velocity[v][0] << ", " << velocity[v][1] << ", "<< velocity[v][2] << std::endl;
}
CurrentTemperature = (2 * KineticEnergy)/(3 * N - 3); // Calculate temperature
TotalEnergy = KineticEnergy + Vfinal;
std::cout<< "ct = " << CurrentTemperature << "spt = " << SetpointTemperature << std::endl;
BussiThermostat(deltaT, CurrentTemperature, SetpointTemperature); // Control temperature
timeEvolved = i * deltaT;
//~ printf ("t, TE, V, KE: %f, %f, %f, %f\n", timeEvolved, TotalEnergy, Vfinal, KineticEnergy);
fprintf (fileVelocityVerlet, "%f, %f, %f, %f\n", timeEvolved, TotalEnergy, Vfinal, KineticEnergy);
if (Vfinal < BestV){
BestV = Vfinal;
for (int v = 0; v < N; v++) {
for (int w = 0; w < 3; w++) {
BestConfiguration[v][w] = position[v][w];
}
}
}
}
CurrentTemperature = (2 * KineticEnergy)/(3 * N - 3);
std::cout << "Final Temperature = " << CurrentTemperature << std::endl
<< "Kinetic Energy = " << KineticEnergy << std::endl
<< "Binding Energy = " << TotalEnergy / N << std::endl
<< "Diffusitivity = " << CalculateDiffusitivity(deltaT, steps) << std::endl;
fprintf (fileBestConfig, "Best Configuration:\n");
for (int i = 0; i < N; i++) {
fprintf (fileBestConfig, "%f, %f, %f\n", BestConfiguration[i][0], BestConfiguration[i][1], BestConfiguration[i][2]);
}
fclose (fileBestConfig);
fclose (fileVelocityVerlet);
timend = time (NULL);
std::cout << (timend-timestart) << " seconds to execute." << std::endl;
//terminate the program
return 0;
}
void FitCurve(SpinDownData s_data[], uint8_t s_index)
{
uint8_t count;
double a, b, c, d;
FittingData f_data[MAX_SPINDOWN_SAMPLES];
count = s_index;
//for (int i = 0; i < s_index; i++)
//{
// //printf("%u, %.2f\n", s_data[i].time, s_data[i].speed);
//}
////printf("\n");
// zero out the array of fitting data and re-populate
memset(f_data, 0, sizeof (f_data));
for (int i = 0; i < count; i++)
{
f_data[i].x = s_data[i].time;
f_data[i].y = s_data[i].speed;
}
// fit the sampled speed vs time data to a quadratic equation (to clean up the data)
if (curve_fit_quad(count, f_data, &a, &b, &c))
{
//printf("fitting failed..\n\n");
}
else
{
//printf("first fitting succeeded..\n");
//printf("fn(x) = a + bx + cx^2\n");
//printf(" a = %lf, b = %lf, c = %lf\n\n", a, b, c);
}
// store & print out the results
for (int i = 0; i < s_index; i++)
{
s_data[i].speed_fitted = a + (b * s_data[i].time) + (c * s_data[i].time * s_data[i].time);
////printf("time = %.2f, original speed = %.2f, fitted speed == %.2f\n", s_data[i].time, s_data[i].speed, s_data[i].speed_fitted);
}
// calculate the stored kinetic energy at each speed point
// = 0.5 * 0.079619 * POWER(C2*0.27777777777778*1000/2098.58*2*PI(),2) + 0.5 * 0.005417523193127 * POWER(C2*0.27777777777778*1000/156.765473414*2*PI(),2)
for (int i = 0; i < s_index; i++)
{
double speed_ms = s_data[i].speed_fitted * 0.27777777777778;
double ke = GetKineticEnergy(speed_ms);
s_data[i].ke = ke;
////printf("kinetic energy @ speed %lf is %lf\n", s_data[i].speed_fitted, s_data[i].ke);
}
// zero out the array of fitting data and re-populate
memset(f_data, 0, sizeof (f_data));
for (int i = 0; i < count; i++)
{
f_data[i].x = s_data[i].time;
f_data[i].y = s_data[i].ke;
}
// fit kinetic energy vs time to a cubic equation
if (curve_fit_cubic(count, f_data, &a, &b, &c, &d))
{
//printf("fitting failed..\n\n");
}
else
{
//printf("second fitting succeeded..\n");
//printf("fn(x) = a + bx + cx^2 + dx^3\n");
//printf(" a = %lf, b = %lf, c = %lf, d = %lf\n\n", a, b, c, d);
}
// store & print out the results
for (int i = 0; i < s_index; i++)
{
s_data[i].ke_fitted = a + (b * s_data[i].time) + (c * s_data[i].time * s_data[i].time) + (d * s_data[i].time * s_data[i].time * s_data[i].time);
////printf("time = %.2f, original ke = %.2f, fitted ke == %.2f\n", s_data[i].time, s_data[i].ke, s_data[i].ke_fitted);
}
// differentiate to get rate of change of KE at a given time
for (int i = 0; i < s_index; i++)
{
s_data[i].ke_rate_of_change = b + (2 * c * s_data[i].time) + (3 * d * s_data[i].time * s_data[i].time);
////printf("time = %.2f, ke rate of change == %.2f\n", s_data[i].time, s_data[i].ke_rate_of_change);
}
// zero out the array of fitting data and re-populate
memset(f_data, 0, sizeof (f_data));
for (int i = 0; i < count; i++)
{
f_data[i].x = s_data[i].speed_fitted;
f_data[i].y = fabs(s_data[i].ke_rate_of_change);
////printf("fitted speed = %lf, fitted load = %lf\n", f_data[i].x, f_data[i].y);
}
// fit rate of change vs speed to a cubic equation
if (curve_fit_cubic(count, f_data, &a, &b, &c, &d))
{
//printf("fitting failed..\n\n");
}
else
{
//printf("third fitting succeeded..\n");
//printf("fn(x) = a + bx + cx^2 + dx^3\n");
//printf(" a = %lf, b = %lf, c = %lf, d = %lf\n\n", a, b, c, d);
//snprintf(DisplayMessage, DISPLAY_MAX_MSG_SIZE, "a = %lf, b = %lf, c = %lf, d = %lf", a, b, c, d);
}
for (int i = 0; i < 70; i++)
{
////printf("speed = %i, fitted load = %lf\n", i, a + (b*i) + (c*i*i) + (d*i*i*i));
}
// use above parameters to model static power requirements
power_curve_a = a;
power_curve_b = b;
power_curve_c = c;
power_curve_d = d;
return;
}
static void *ANT_Thread(void *arg)
{
struct TimerInfo info;
uint8_t power_event_count = 0;
uint16_t power_accumulated = 0;
uint16_t power_instant = 0;
double previous_ke = 0;
uint8_t power_accel_index;
double power_accel_array[POWER_SAMPLE_DEPTH];
double power_accel_filtered;
// zero out the array of power entries & set index to start
memset(power_accel_array, 0, sizeof (power_accel_array));
power_accel_index = 0;
// USB setup
if (libusb_init(NULL) != 0)
{
//printf("libusb: failed to initialise\n");
snprintf(DisplayMessage, DISPLAY_MAX_MSG_SIZE, "libusb: failed to initialise...");
return NULL;
}
devh = libusb_open_device_with_vid_pid(NULL, vendor_id, product_id);
if (devh == NULL)
{
//printf("libusb: failed to open device 0x%04x:0x%04x\n", vendor_id, product_id);
snprintf(DisplayMessage, DISPLAY_MAX_MSG_SIZE, "libusb: failed to open device 0x%04x:0x%04x\n", vendor_id, product_id);
}
else
{
// don't really care about the result..
libusb_detach_kernel_driver(devh, 0);
if (libusb_claim_interface(devh, 0) != 0)
{
//printf("libusb: failed to claim interface");
snprintf(DisplayMessage, DISPLAY_MAX_MSG_SIZE, "libusb: failed to claim interface");
}
else
{
//printf("libusb: succesfully claimed interface\n");
snprintf(DisplayMessage, DISPLAY_MAX_MSG_SIZE, "libusb: succesfully claimed interface");
// ANT+ setup
// reset
ANTBuildMessage(1, ANT_SYSTEM_RESET, 0, 0, 0, 0, 0, 0, 0, 0, 0);
ANTTransferMessage();
// ANT docs say wait 500ms after reset before sending any more host commands
milliSleep(500);
// set network key
ANTBuildMessage(9, ANT_SET_NETWORK, ANT_NETWORK_0, key[0], key[1], key[2], key[3], key[4], key[5], key[6], key[7]);
ANTTransferMessage();
// assign channel
ANTBuildMessage(3, ANT_ASSIGN_CHANNEL, ANT_CHANNEL_1, ANT_CHANNEL_TYPE_TX, ANT_NETWORK_0, 0, 0, 0, 0, 0, 0);
ANTTransferMessage();
// set channel id
uint16_t device_id = 0xBEEF;
ANTBuildMessage(5, ANT_CHANNEL_ID, ANT_CHANNEL_1, device_id&0xff, (device_id>>8)&0xff, ANT_SPORT_POWER_TYPE, ANT_TRANSMISSION_TYPE, 0, 0, 0, 0);
ANTTransferMessage();
// set rf frequency
ANTBuildMessage(2, ANT_CHANNEL_FREQUENCY, ANT_CHANNEL_1, ANT_SPORT_FREQUENCY, 0, 0, 0, 0, 0, 0, 0);
ANTTransferMessage();
// set channel period
uint16_t period = ANT_SPORT_POWER_PERIOD;
ANTBuildMessage(3, ANT_CHANNEL_PERIOD, ANT_CHANNEL_1, period&0xff, (period>>8)&0xff, 0, 0, 0, 0, 0, 0);
ANTTransferMessage();
// set tx power? (optional)
// open channel
ANTBuildMessage(1, ANT_OPEN_CHANNEL, ANT_CHANNEL_1, 0, 0, 0, 0, 0, 0, 0, 0);
ANTTransferMessage();
// Start the periodic timer @ 250ms
// todo: use stricter ANT power message interval?
TimerStart (250000, &info);
//TESTING!!!
// ONCE PER SECOND TO MATCH SPEED UPDATES
//TimerStart (1000000, &info);
// Run until terminated from main thread
while (runAntThread)
{
// Wait for periodic timer to expire
TimerWait (&info);
// Broadcast data
////printf("ANT_Thread: timer expired...\n");
// Data Page 0x10 message format
// Byte 0 : Data Page Number - 0x10
// Byte 1 : Update event count
// Byte 2 : Pedal power - 0xFF
// Byte 3 : Instantaneous cadence - 0xFF
// Byte 4 : Accumulated power (LSB)
// Byte 5 : Accumulated power (MSB)
// Byte 6 : Instantaneous power (LSB)
// Byte 7 : Instantaneous power (MSB)
// #define POWER 150
// power_instant = POWER;
double speed = currentSpeed;
// calculate the static power
double power_static = (power_curve_a +
(power_curve_b*speed) +
(power_curve_c*speed*speed) +
(power_curve_d*speed*speed*speed));
// calculate the kinetic energy at this speed
double speed_ms = speed / 3.6;
double current_ke = GetKineticEnergy(speed_ms);
// calculate the acceleration power. This calculation is dependent on the update
// frequency, as we are looking for the change in stored kinetic energy per second
//
double power_accel = (current_ke - previous_ke) * PRU_UPDATE_FREQ;
// todo: this may need some smoothing as the frequency increases?
// - start with a simple moving average of the acceleration power
power_accel_array[power_accel_index++] = power_accel;
if (power_accel_index >= POWER_SAMPLE_DEPTH)
{
power_accel_index = 0;
}
power_accel_filtered = 0;
for (int i = 0; i < POWER_SAMPLE_DEPTH; i++)
{
power_accel_filtered += power_accel_array[i];
}
power_accel_filtered /= POWER_SAMPLE_DEPTH;
//snprintf(DisplayMessage, DISPLAY_MAX_MSG_SIZE, "current_ke = %lf, previous_ke = %lf", total_kinetic_energy, previous_ke);
//snprintf(DisplayMessage, DISPLAY_MAX_MSG_SIZE, "power_static = %.0lf, power_accel = %.0lf", power_static, power_accel);
// Print out the value received from the PRU code
//printf("%u events, %.0f RPM, %.2f km/h, %.2f watts\r\n", events, rpm, kph, power_static+power_accel);
previous_ke = current_ke;
power_event_count++;
power_instant = power_static + power_accel_filtered;
currentPower = power_instant;
power_accumulated += power_instant;
ANTBuildMessage(9, ANT_BROADCAST_DATA, ANT_CHANNEL_1, ANT_STANDARD_POWER, power_event_count, 0xFF, 0xFF,
power_accumulated&0xff, (power_accumulated>>8)&0xff, power_instant&0xff, (power_instant>>8)&0xff);
ANTTransferMessage();
}
// ANT+ cleanup
//printf("ANT_Thread: cleanup\n");
// close channel
ANTBuildMessage(1, ANT_CLOSE_CHANNEL, ANT_CHANNEL_1, 0, 0, 0, 0, 0, 0, 0, 0);
ANTTransferMessage();
// USB cleanup
libusb_release_interface(devh, 0);
}
// USB cleanup
libusb_close(devh);
}
libusb_exit(NULL);
return NULL;
}
