Code:
const int ledPin = 13; //LED connected to digital pin 13
const int clockPin = 7; //output to clock
const int CSnPin = 6; //output to chip select
const int inputPin = 2; //read AS5040
int inputstream = 0; //one bit read from pin
long packeddata = 0; //two bytes concatenated from inputstream
long angle = 0; //holds processed angle value
long anglemask = 65472; //0x1111111111000000: mask to obtain first 10 digits with position info
long statusmask = 63; //0x000000000111111; mask to obtain last 6 digits containing status info
long statusbits; //holds status/error information
int DECn; //bit holding decreasing magnet field error data
int INCn; //bit holding increasing magnet field error data
int OCF; //bit holding startup-valid bit
int COF; //bit holding cordic DSP processing error data
int LIN; //bit holding magnet field displacement error data
int debug = 1; //SET THIS TO 0 TO DISABLE PRINTING OF ERROR CODES
int shortdelay = 80; // this is the microseconds of delay in the data clock
int longdelay = 8; // this is the milliseconds between readings
int pwm_pin = 3; //Motor pwm
int in_1 = 4; //Motor Richtung
int in_2 = 5; //Motor Richtung
String msg;
char a;
int soll_wert;
int ist_wert;
int e;
int Winkel;
int Winkel_alt;
int Winkel_abweichung;
int Um;
int esum;
int ealt;
int yaus;
int y_max = 128;
int y_min = 0;
int esum_max = 700;
float y;
float i;
float d;
float Kp = 2.0; //P
float Ki = 3.0; //I
float Kd = 0.5; //D
float Ta = 0.02; //Abtast
void setup()
{
Serial.begin(9600);
pinMode(ledPin, OUTPUT); // visual signal of I/O to chip
pinMode(clockPin, OUTPUT); // SCK
pinMode(CSnPin, OUTPUT); // CSn -- has to toggle high and low to signal chip to start data transfer
pinMode(inputPin, INPUT); // SDA
pinMode(in_1, OUTPUT);
pinMode(in_2, OUTPUT);
pinMode(pwm_pin, OUTPUT);
digitalWrite(in_1, HIGH);
digitalWrite(in_2, LOW);
analogWrite(pwm_pin, 0);
// CSn needs to cycle from high to low to initiate transfer. Then clock cycles. As it goes high
// again, data will appear on sda
digitalWrite(CSnPin, HIGH); // CSn high
digitalWrite(clockPin, HIGH); // CLK high
delay(longdelay);
digitalWrite(ledPin, HIGH); // signal start of transfer with LED
digitalWrite(CSnPin, LOW); // CSn low: start of transfer
delayMicroseconds(shortdelay); // delay for chip -- 1000x as long as it needs to be
digitalWrite(clockPin, LOW); // CLK goes low: start clocking
delayMicroseconds(shortdelay); // hold low for 10 ms
for (int x=0; x <16; x++) // clock signal, 16 transitions, output to clock pin
{
digitalWrite(clockPin, HIGH); //clock goes high
delayMicroseconds(shortdelay); // wait 10ms
inputstream =digitalRead(inputPin); // read one bit of data from pin
//Serial.print(inputstream, DEC);
packeddata = ((packeddata << 1) + inputstream);// left-shift summing variable, add pin value
digitalWrite(clockPin, LOW);
delayMicroseconds(shortdelay); // end of one clock cycle
}
// end of entire clock cycle
//Serial.println(" ");
digitalWrite(ledPin, LOW); // signal end of transmission
// lots of diagnostics for verifying bitwise operations
//Serial.print("packed:");
//Serial.println(packeddata,DEC);
//Serial.print("pack bin: ");
// Serial.println(packeddata,BIN);
angle = packeddata & anglemask; // mask rightmost 6 digits of packeddata to zero, into angle.
//Serial.print("mask: ");
//Serial.println(anglemask, BIN);
//Serial.print("bin angle:");
//Serial.println(angle, BIN);
//Serial.print("angle: ");
//Serial.println(angle, DEC);
angle = (angle >> 6); // shift 16-digit angle right 6 digits to form 10-digit value
//Serial.print("angleshft:");
//Serial.println(angle, BIN);
//Serial.print("angledec: ");
//Serial.println(angle, DEC);
angle = angle * 0.3515; // angle * (360/1024) == actual degrees
//Serial.print("angle: "); // and, finally, print it.
Winkel_alt = int(angle);
//Serial.println("--------------------");
//Serial.print("raw: "); // this was the prefix for the bit-by-bit diag output inside the loop.
if (debug)
{
statusbits = packeddata & statusmask;
DECn = statusbits & 2; // goes high if magnet moved away from IC
INCn = statusbits & 4; // goes high if magnet moved towards IC
LIN = statusbits & 8; // goes high for linearity alarm
COF = statusbits & 16; // goes high for cordic overflow: data invalid
OCF = statusbits & 32; // this is 1 when the chip startup is finished.
if (DECn && INCn) { Serial.println("magnet moved out of range"); }
else
{
if (DECn) { Serial.println("magnet moved away from chip"); }
if (INCn) { Serial.println("magnet moved towards chip"); }
}
if (LIN) { Serial.println("linearity alarm: magnet misaligned? Data questionable."); }
if (COF) { Serial.println("cordic overflow: magnet misaligned? Data invalid."); }
}
packeddata = 0; // reset both variables to zero so they don't just accumulate
angle = 0;
}
void loop()
{
//unsigned long startmicros=micros();
if(Serial.available() > 0)
{
msg = "";
while(Serial.available() > 0){
a = Serial.read();
msg = msg + String(a);
delay(250);
}
soll_wert = msg.toInt();
Serial.println(soll_wert);
}
// CSn needs to cycle from high to low to initiate transfer. Then clock cycles. As it goes high
// again, data will appear on sda
digitalWrite(CSnPin, HIGH); // CSn high
digitalWrite(clockPin, HIGH); // CLK high
delay(longdelay);
digitalWrite(ledPin, HIGH); // signal start of transfer with LED
digitalWrite(CSnPin, LOW); // CSn low: start of transfer
delayMicroseconds(shortdelay); // delay for chip -- 1000x as long as it needs to be
digitalWrite(clockPin, LOW); // CLK goes low: start clocking
delayMicroseconds(shortdelay); // hold low for 10 ms
for (int x=0; x <16; x++) // clock signal, 16 transitions, output to clock pin
{
digitalWrite(clockPin, HIGH); //clock goes high
delayMicroseconds(shortdelay); // wait 10ms
inputstream =digitalRead(inputPin); // read one bit of data from pin
//Serial.print(inputstream, DEC);
packeddata = ((packeddata << 1) + inputstream);// left-shift summing variable, add pin value
digitalWrite(clockPin, LOW);
delayMicroseconds(shortdelay); // end of one clock cycle
}
// end of entire clock cycle
//Serial.println(" ");
digitalWrite(ledPin, LOW); // signal end of transmission
// lots of diagnostics for verifying bitwise operations
//Serial.print("packed:");
//Serial.println(packeddata,DEC);
//Serial.print("pack bin: ");
// Serial.println(packeddata,BIN);
angle = packeddata & anglemask; // mask rightmost 6 digits of packeddata to zero, into angle.
//Serial.print("mask: ");
//Serial.println(anglemask, BIN);
//Serial.print("bin angle:");
//Serial.println(angle, BIN);
//Serial.print("angle: ");
//Serial.println(angle, DEC);
angle = (angle >> 6); // shift 16-digit angle right 6 digits to form 10-digit value
//Serial.print("angleshft:");
//Serial.println(angle, BIN);
//Serial.print("angledec: ");
//Serial.println(angle, DEC);
angle = angle * 0.3515; // angle * (360/1024) == actual degrees
//Serial.print("angle: "); // and, finally, print it.
//Serial.println(angle, DEC);
Winkel = int(angle);
//Serial.println("--------------------");
//Serial.print("raw: "); // this was the prefix for the bit-by-bit diag output inside the loop.
if (debug)
{
statusbits = packeddata & statusmask;
DECn = statusbits & 2; // goes high if magnet moved away from IC
INCn = statusbits & 4; // goes high if magnet moved towards IC
LIN = statusbits & 8; // goes high for linearity alarm
COF = statusbits & 16; // goes high for cordic overflow: data invalid
OCF = statusbits & 32; // this is 1 when the chip startup is finished.
if (DECn && INCn) { Serial.println("magnet moved out of range"); }
else
{
if (DECn) { Serial.println("magnet moved away from chip"); }
if (INCn) { Serial.println("magnet moved towards chip"); }
}
if (LIN) { Serial.println("linearity alarm: magnet misaligned? Data questionable."); }
if (COF) { Serial.println("cordic overflow: magnet misaligned? Data invalid."); }
}
packeddata = 0; // reset both variables to zero so they don't just accumulate
angle = 0;
regler();
//unsigned long endmicros=micros();
//Serial.println(endmicros-startmicros,DEC);
}
void regler() {
Winkel_abweichung = Winkel_alt - Winkel;
if(Winkel_abweichung > 180) {
Um = Um + 1;
}
if(Winkel_abweichung < -180) {
Um = Um - 1;
}
ist_wert = 360 * Um;
ist_wert = Winkel + ist_wert;
Winkel_alt = Winkel;
Serial.println(ist_wert);
e = soll_wert - ist_wert;
e = abs(e);
esum = esum + e;
if(esum > esum_max) {
esum = esum_max;
}
i = float(esum) * Ta;
i = i * Ki;
d = float(e) - float(ealt);
d = d / Ta;
d = d * Kd;
y = Kp * e;
y = y + i;
y = y + d;
yaus = int(y);
//Serial.println(y);
ealt = e;
if(yaus > y_max) {
yaus = y_max;
}
if(yaus < y_min) {
yaus = y_min;
}
if(ist_wert > soll_wert) {
digitalWrite(in_1, HIGH);
digitalWrite(in_2, LOW);
analogWrite(pwm_pin, yaus);
}
if(ist_wert < soll_wert) {
digitalWrite(in_1, LOW);
digitalWrite(in_2, HIGH);
analogWrite(pwm_pin, yaus);
}
if(ist_wert == soll_wert) {
digitalWrite(in_1, LOW);
digitalWrite(in_2, LOW);
analogWrite(pwm_pin, 0);
esum = 0;
ealt = 0;
}
}
Zitieren
Lesezeichen