Hi guys,
I’m really breaking my head on this one. I’ve written/adapted a script that reads a inclinometer analogue output through a 22 bit ADC by a SPI bit banging method. The sketch works and is fast!, but…
I also wrote/adapted a sketch that uses a TLC4940 16 channel PWM unit to control a stepper motor. The stepper motor can be micro stepped this way. Each function call takes only 150 microseconds. The script works and is fast! but…
I also wrote/adapted a small PID controller sketch, that takes the output from the 22 bit ADC and calculates a input for the stepper motor TCL4940 function. Actually it’s a really simple function because it only uses the Proportional parameter and no floating types. The function works and is fast but…
When I combine these three scripts, it get’s really slow! When I read the serial monitor one or two values are spit out, then there is a delay, than there are 4 or 5 values spit out fast after each other and then again a delay. Then maybe again 1 or 2 values and so on…
It seems that the 3 different sketches aren’t compatible and this results in cloghing up the system. I posted the sketch below. I hope somebody can give me some hints what is happening here.
Thanks in advance!
/* Two inclinometers are read by two 22 bit ADC's. The
output is fed to a PID function which calculates the
input for the stepper motor functions. The ADC's
are accessed through a SPI interface by a bit banging method.
The two stepper motors are controlled by PWM through a
TLC5940 driver.
In this version only 1 ADC and 1 stepper motor is driven
by Richard Lekkerkerker
Rotterdam, 03-06-2010
*/
#include "Tlc5940.h"
/*Global variables ADCs*/
int ChipA_Select = 8;
int ChipB_Select = 12;
int From_Data_Out = 5;
int Sclk = 2;
int To_Data_In = 4;
int analog_pin = 1;
int analog_value = 0;
int i = 0;
long sensor_waardeA = 0;
long sensor_waardeB = 0;
/*Global variables stepper motors*/
int ThrottleA = 0;
int ThrottleB = 0;
int ChannelA = 0;
int ChannelB = 0;
/*Global variables PID*/
long target_A = -47000;
int lastError_A = 0;
int sumError_A = 0;
long previousTarget = 0;
int iMax = 100; //Integral term min/max (random value and not yet tested/verified)
int iMin = 0;
/*PID controller constants*/
int KP = 3; //position multiplier (gain)
// float KI = .25; // Intergral multiplier (gain)
// float KD = 1.0; // derivative multiplier (gain)
void setup()
{
pinMode(ChipA_Select, OUTPUT);
digitalWrite(ChipA_Select, LOW); // extra
pinMode(ChipB_Select, OUTPUT);
digitalWrite(ChipB_Select, LOW); // extra
pinMode(From_Data_Out, INPUT);
pinMode(Sclk, OUTPUT);
digitalWrite(Sclk, HIGH); // extra
pinMode(To_Data_In, OUTPUT);
Serial.begin(57600);
Serial.println("De sketch is geladen...");
Tlc.init(0);
}
void loop()
{
chip_select_low(ChipA_Select);
chip_select_high(ChipA_Select);
sensor_waardeA = read_value(ChipA_Select);
docalc_A();
//Serial.print("The value of sensor A is: ");
//Serial.print(sensor_waardeA, DEC);
//Serial.print(" ");
//Serial.print("The value of sensor B is: ");
//Serial.println(sensor_waardeB, DEC);
}
/********************ADC reading functions**************************/
void chip_select_low(int chip) //Function to tell the ADC that the Arduino wants to connect to the ADC
{
digitalWrite(chip, LOW);
}
void chip_select_high(int chip) //Function to tell the ADC that the Arduino wants to DISconnect to the ADC
{
digitalWrite(chip, HIGH);
}
void sclk() //Clock function
{
digitalWrite(Sclk, LOW);
digitalWrite(Sclk, HIGH);
}
/*******************************************************/
long read_value(int chip) //Function to read the ADC bit by bit through a SPI bit banging value and save to 'svalue'
{
int bit1;
char readybit= 1; // extra
int index;
char ovlbit = 0; //extra
char ovhbit = 0; //extra
char signbit = 0; //extra
unsigned long uvalue;
long svalue;
unsigned long temp;
unsigned long one = 1;
int btime;
int time;
// btime = micros();
while (readybit == 1)
{
chip_select_low(chip);
readybit = digitalRead(From_Data_Out); //extra
UpdateNanostep();
if (readybit == 0)
break;
chip_select_high(chip);
}
// time = micros() - btime;
// Serial.println(time, DEC);
chip_select_low(chip);
// readybit = digitalRead(From_Data_Out);
// if (readybit == 1)
// {
// Serial.println("ADC is not done and is still converting the 22bit value");
// }
sclk();
ovlbit = digitalRead(From_Data_Out);
sclk();
ovhbit = digitalRead(From_Data_Out);
sclk();
signbit = digitalRead(From_Data_Out);
uvalue = 0;
for (index = 0; index < 21; index++) // each bit is read seperatly, the MSB first
{
sclk();
bit1 = digitalRead(From_Data_Out);
if (bit1 == HIGH)
{
temp = (one << (20 - index));
temp = temp & 0x001FFFFF;
uvalue = uvalue | temp;
}
}
if (signbit == 1)
{
svalue = -((~uvalue & 0x001FFFFF) + 1);
}
if (ovlbit == 1)
{
svalue = -2097153;
}
if (ovhbit == 1)
{
svalue = 2097152;
}
chip_select_high(chip);
return svalue;
}
/********************Stepper motor functions*******************************/
void UpdateNanostep()
{
int A;
int B;
int i;
ChannelA = ChannelA + ThrottleA;
if (ChannelA >= 16380){ChannelA = ChannelA - 16380;};
if (ChannelA < 0){ChannelA = ChannelA + 16380;};
A = ChannelA % 4095;
B = 4095 - A;
i = (ChannelA - A)/4095; //index
switch (i) {
case 0:
Tlc.set(3, B);
Tlc.set(4, 4095);
Tlc.set(5, A);
Tlc.set(6, 0);
break;
case 1:
Tlc.set(3, 0);
Tlc.set(4, B);
Tlc.set(5, 4095);
Tlc.set(6, A);
break;
case 2:
Tlc.set(3, A);
Tlc.set(4, 0);
Tlc.set(5, B);
Tlc.set(6, 4095);
break;
case 3:
Tlc.set(3, 4095);
Tlc.set(4, A);
Tlc.set(5, 0);
Tlc.set(6, B);
break;
};
ChannelB = ChannelB + ThrottleB;
if (ChannelB >= 16380){ChannelB = ChannelB - 16380;};
if (ChannelB < 0){ChannelB = ChannelB + 16380;};
A = ChannelB % 4095;
B = 4095 - A;
i = (ChannelB - A)/4095; //index
switch (i) {
case 0:
Tlc.set(9, B);
Tlc.set(10, 4095);
Tlc.set(11, A);
Tlc.set(12, 0);
break;
case 1:
Tlc.set(9, 0);
Tlc.set(10, B);
Tlc.set(11, 4095);
Tlc.set(12, A);
break;
case 2:
Tlc.set(9, A);
Tlc.set(10, 0);
Tlc.set(11, B);
Tlc.set(12, 4095);
break;
case 3:
Tlc.set(9, 4095);
Tlc.set(10, A);
Tlc.set(11, 0);
Tlc.set(12, B);
break;
};
while(Tlc.update());
}
/********************PID functions*****************************************/
void docalc_A() {
long error_A = sensor_waardeA - target_A ; // find the error term of current position - target
//generalized PID formula
// long ms_A = KP * error_A + KD * (error_A - lastError_A) + KI * (sumError_A);
long ms_A = KP * error_A;
// Serial.print("the value of Proportional is: ");
// Serial.print(KP * error_A, DEC);
lastError_A = error_A;
sumError_A += error_A;
//scale the sum for the integral term
if(sumError_A > iMax) {
sumError_A = iMax;
} else if(sumError_A < iMin){
sumError_A = iMin;
}
Serial.print("The value of ms_A is: ");
Serial.print(ms_A, DEC);
// long motorSpeed_A = map(ms_A, -0, 2000000, 0, 3072);
long motorSpeed_A = ms_A / 300000; //Value from ADC is divided to attain the right stepper motor throttle input signal
ThrottleA = -motorSpeed_A;
Serial.print("The value of Throttle A is: ");
Serial.println(ThrottleA, DEC);
// }
}