Hello all,
I am using the duemilinove with arduino 0018. I would like to have my loop time to be as fast as possible. However, I noticed that I must have a delay at the end of my loop. I am using timer 1 (ie. pins 3 and 11) as pwm output.
I am using the millis function and storing one loop begin time in an array and the second loop begin time in an array and I calculate delta t. I noticed that dt = 0 when I have no delay in the loop. So I suppose that the delay function somehow affects timer1? But I changed to different pins on different timers and the same problem persists.
I doubt the code will reveal anything but I will place it in here if any of you are curious. Be warned, it is very messy code as of this moment.
Your help is greatly appreciated.
int anglesensorPin = 1; //angle sensor input
int positionsensorPin = 5; //pot input
int angle_sensor_reading = 0; // ADC value from angle --> 10 bit conversion
int position_sensor_reading = 0; //same as angle ADC
float deg = 0; //stores degrees
float x = 0; //stores
//float position = 0;
int motorPin1 = 3; //output to motor lead
int motorPin2 = 11; //output to motor lead
//int motorPower = 4; //don't need as motor has its own supply
float ang_velocity_array[2] = {0,0}; //stores average of thetadot
float x_velocity_array[2] = {0,0}; //ditto for xdot
float time_array[2] = {0,0}; //stores time for dt calc
unsigned long time = 0; //stores total time
int count = 0; //counting variable for dt,dx,dtheta arrays
float dtheta;
float dx;
float dt;
float thetadot;
float xdot;
float V_hbridge; //LQR voltage
float percent_duty_cycle; //pwm in percentage from V_hbridge (eg.20% not 0.2)
int output_to_motor; //outputs LQR voltage to motor
float a = -17.8837;
float b = -27.3602;
float c = 59.3220;
float d = 16.03;
const int numReadings = 40; //index for averaging filter
int readings[numReadings]; // the readings from the analog input
int index = 0; // the index of the current reading
float xTot = 0; // the running total
float xAv = 0; // the average
float thetTot = 0; //ditto for theta
float thetAv = 0; //ditto for theta
const int numvelocityReadings = 40;
int velocity_avg_index = 0; //ditto for velocities
float xdot_readings[numvelocityReadings];
float xdotTot = 0;
float xdotAv = 0;
float thetadot_readings[numvelocityReadings];
float thetadotTot = 0;
float thetadotAv = 0;
//int test = 7;
void setup()
{
// declare the ledPin as an OUTPUT:
pinMode(positionsensorPin, INPUT);
pinMode(anglesensorPin, INPUT);
pinMode(motorPin1, OUTPUT);
pinMode(motorPin2, OUTPUT);
//pinMode(motorPower, OUTPUT);
//pinMode(test, OUTPUT);
TCCR2A = _BV(COM2A1) | _BV(COM2B1) | _BV(WGM21) | _BV(WGM20);
TCCR2B = _BV(CS21); //changes PWM frequency by dividing 16MHz by prescalar value
//initialize averaging cells to zero once power is on
for(int thisReading = 0; thisReading < numReadings; thisReading++)
{
readings[thisReading] = 0;
xdot_readings[thisReading] = 0;
}
Serial.begin(9600);
}
void loop()
{
//digitalWrite(test, HIGH);
time = millis(); //counts time since power on in ms
time_array[count] = time; //stores the time
//averaging filter for theta
thetTot = thetTot - readings[index];
readings[index] = analogRead(anglesensorPin);
thetTot = thetTot + readings[index];
index = index + 1;
//averaging filter for X
xTot = xTot - readings[index];
readings[index] = analogRead(positionsensorPin);
xTot = xTot + readings[index];
index = index + 1;
//resets the cells once up to number of samples
if(index >= numReadings)
{
index = 0;
}
//calculates the average
thetAv = thetTot / numReadings;
xAv = xTot / numReadings;
angle_sensor_reading = thetAv; //define the angle sensor reading to the average of theta
//Serial.println(angle_sensor_reading);
deg = (angle_sensor_reading * 0.9045 - 73.247 - 180 + 4.51) * (3.14/180); //calibration of theta
ang_velocity_array[count] = deg; //stores theta for caluclation of dtheta
//Serial.println(deg);
position_sensor_reading = xAv; //ditto for xdot
x = ((position_sensor_reading*0.2294) - 8.0056 - 31.65) / 100; //ditto for xdot
x_velocity_array[count] = x; //ditto for xdot
//Serial.println(x);
//if the counting variable dtheta,dx,dt is > 1, takes another measurement of theta,x, t
if(count < 1)
{
count++;
}
else
{
count = 0; //reset counting variable
//calculates dtheta,dx,dt
dtheta = (ang_velocity_array[0] - ang_velocity_array[1]);
dx = (x_velocity_array[0] - x_velocity_array[1]);
dt = (abs(time_array[0] - time_array[1])) / 1000;
//calculates thetadot, xdot
thetadot = (dtheta/dt);
xdot = (dx/dt);
//Serial.println(dt);
//averaging filter for xdot
xdotTot = xdotTot - xdot_readings[velocity_avg_index];
xdot_readings[velocity_avg_index] = xdot;
xdotTot = xdotTot + xdot_readings[velocity_avg_index];
//averaging filter for thetadot
thetadotTot = thetadotTot - thetadot_readings[velocity_avg_index];
thetadot_readings[velocity_avg_index] = xdot;
thetadotTot = thetadotTot + thetadot_readings[velocity_avg_index];
//same indexing idea as average for x and theta
velocity_avg_index = velocity_avg_index + 1;
//Serial.println(xdot);
//same indexing idea as average for x and theta
if(velocity_avg_index >= numvelocityReadings)
{
velocity_avg_index = 0;
}
//ISR HERE?
//calculates average for xdot and thetadot
xdotAv = xdotTot / numvelocityReadings;
thetadotAv = thetadotTot / numvelocityReadings;
//Serial.println(xdotAv);
//LQR equation
V_hbridge = (a*x_velocity_array[1] + b*xdotAv + c*ang_velocity_array[1] + d*thetadotAv);
percent_duty_cycle = 11.894 * V_hbridge;//set zero intercept
output_to_motor = abs(((percent_duty_cycle/100) * 256) + 1);
Serial.println(V_hbridge);
//Output to motor lead depending on V_hbridge polarity
if(V_hbridge < 0)
{
//output_to_motor = 1.25*output_to_motor;
//output_to_motor = output_to_motor + 50;
analogWrite(motorPin2, output_to_motor);
}
else
{
//output_to_motor = 2*output_to_motor;
//output_to_motor = output_to_motor + 25;
analogWrite(motorPin1, output_to_motor);
}
//ISR?
x_velocity_array[0] = 0;
x_velocity_array[1] = 0;
time_array[0] = 0;
time_array[1] = 0; //was [0]??
}
//digitalWrite(test, LOW);
//Serial.println(time);
delay(1);
}