Hi everyone,
I have a problem with a program/sketch that somehow manages to reset the atmega when presented with a certain serial input.
This program is supposed to control the position of a brushed dc motor using a quadrature encoder. The pwm output is controlled by the PIDLibrary found on arduino.cc.
After power up, the motor will rotate 180°CCW and 360°CW to locate the encoder Z-marker. After the marker is found the motor rotates to the standby position by setting requestedEncoderPos to 2048.
To set the motor position a short message is sent via the serial connection. For example, the command 'a2048' followed by carriage return should position the motor at 180°/2048 encoder transitions.
The problem is that i can send values from 0 (a0) to 999 (a999) and the system responds as expected. But if I send 'a1000' or any value above 999 the microcontroller apparently crashes/resets and starts the init cycle as if it was powercycled...
I have tried to locate the error point and it looks like that the line 'requestedEncoderPos = incomingNumber;' gets executed successfully (I can read out requestedEncoderPos by sending the cmd 'a' only). But as soon as the motor starts to move the uc crashes. (I disconnected the motor wires and discovered that the uc crashed when the encoder disc moved i.e. generated an interrupt).
Can anyone explain why the uc crashes when I send 6 serial bytes, but not when I send 5 - and how this relates to a crash when an interrupt is generated?
Another thing, if I hardcode the value by replacing the incomingNumber variable with eg. 1234 the problem disappears. This makes me believe that there is some problem with the number/datatype conversion - but i don't understand how this relates to the generated interrupt?
Could someone take a look at my sketch and hopefully point out any errors I've made?
Thanks!
#include <PID_v1.h>
enum PinAssignments {
encoderPinA = 2,
encoderPinB = 3,
encoderPinZ = 8,
motorPlus = 9,
motorMinus = 10,
laserPin = 7,
lightPin = 11,
};
boolean encoderZero = false; //indicates if the scanner has been initialized
//byte mode = 0; //0=init, 1=angle, 2=speed
volatile double encoderPos = 0; // volatile as value is modified in interrupt routine
double pidEncoderPos = 0; // same value as encoderPos
double requestedEncoderPos = -2048; // turn 180deg ccw
// motor speed reference
double motorRef = 0; // pwm value from pid controller
double lastMotorRef = 0; // current pwm value
// encoder states
boolean A_set = false;
boolean B_set = false;
// pid values
double kp = 3;
double ki = 0;
double kd = 0;
double outputLimit = 255;
// define pid controller
PID myPID(&pidEncoderPos, &motorRef, &requestedEncoderPos, kp, ki, kd, DIRECT);
// incoming serial data
byte incomingByte;
byte SerialInBufCnt = 0; // contains the max no of bytes in the buffer
byte SerialInBuf[5] = {0};
void setup() {
// set pwm frequency
//TCCR1B &= ~(_BV(CS10)); // clear CS10
TCCR1B &= ~(_BV(CS11)); // clear CS11
TCCR1B |= _BV(CS12); // set CS12
// CS12 CS11 CS10
// 0 0 1 No prescale (16MHz)(32kHz pwm)
// 0 1 0 /8 (2MHz) (3921Hz pwm) high jitter, high pitch noise
// > 0 1 1 /64 (250kHz) (490Hz pwm) medium jitter, noisy
// 1 0 0 /256 (62.5kHz) (123Hz pwm) medium jitter, low audio
// 1 0 1 /1024 (15625Hz) (31Hz pwm) low jitter, no audio <--
// configure encoder inputs
pinMode(encoderPinA, INPUT);
pinMode(encoderPinB, INPUT);
pinMode(encoderPinZ, INPUT);
//digitalWrite(encoderPinA, LOW); // turn off pullup resistor
//digitalWrite(encoderPinB, LOW);
//digitalWrite(encoderPinZ, LOW);
// configure motor outputs
pinMode(motorPlus, OUTPUT); // Motor cable +
pinMode(motorMinus, OUTPUT); // Motor cable -
//digitalWrite(motorPlus, LOW); // disable motor
//digitalWrite(motorMinus, LOW);
// configure laser and light
pinMode(laserPin, OUTPUT);
pinMode(lightPin, OUTPUT);
// encoder pin on interrupt 0 (pin 2)
attachInterrupt(0, doEncoderA, CHANGE);
// encoder pin on interrupt 1 (pin 3)
attachInterrupt(1, doEncoderB, CHANGE);
// start serial connection
Serial.begin(115200);
// setup pid controller
myPID.SetOutputLimits(0-outputLimit,outputLimit);
myPID.SetSampleTime(60);
myPID.SetMode(AUTOMATIC);
} //end setup()
void setMotorRef(double _ref){
if (_ref < kp) { // hysteresis
_ref = abs(_ref); // invert negative values
digitalWrite(motorPlus, LOW); // sink current through plus
analogWrite(motorMinus, _ref); // output pwm on minus
} else if (_ref > kp) { // hysteresis
digitalWrite(motorMinus, LOW); // sink current through minus
analogWrite(motorPlus, _ref); // output pwm on plus
} else { // don't run motor if reference is below threshold = kp
digitalWrite(motorMinus, LOW);
digitalWrite(motorPlus, LOW);
positionAchieved();
}
}
void positionAchieved() {
if (encoderZero == false) { // init position mode
requestedEncoderPos = 4096; //turn 360 cw
}
}
void loop(){
// when zero marker passes sensor - reset encoderpos
if (encoderZero == false && digitalRead(encoderPinZ) == LOW) {
encoderPos = 0; // zero encoder counter
requestedEncoderPos = 2048; // goto standby position
encoderZero = true; // scanner ready
Serial.println("Ready");
}
pidEncoderPos = encoderPos; // fix to overcome volatile double* to double* problem(why?)
myPID.Compute(); // calulate pid output
// if motor speed reference changed by pid, send change to motor
if (lastMotorRef != motorRef) {
setMotorRef(motorRef);
}
if (Serial.available() > 0) {
incomingByte = Serial.read(); // read serial buffer
SerialInBuf[SerialInBufCnt] = incomingByte; // add byte to buffer
if (incomingByte == 13) {
switch (SerialInBuf[0]) {
case 'a': // angle
if (SerialInBufCnt > 1) {
//contains data
double incomingNumber = 0;
//parse data
for (byte i=1; i<SerialInBufCnt; i++) {
if (SerialInBuf[i] >= 48 && SerialInBuf[i] <= 57) {
incomingNumber = incomingNumber * 10 + (SerialInBuf[i]-48);
}
}
// update position
Serial.println(requestedEncoderPos);
requestedEncoderPos = incomingNumber;
Serial.println(requestedEncoderPos);
//incomingNumber = 0;
}
else {
Serial.print(encoderPos);
Serial.print(" ");
Serial.print(requestedEncoderPos);
Serial.print(" ");
Serial.print(motorRef);
Serial.print(" ");
Serial.println(millis());
}
break;
case 'f': // flashlight
// cmd
break;
case 'l': // laser
if (SerialInBufCnt > 1) {
switch (SerialInBuf[1]) {
case 49:
digitalWrite(laserPin, HIGH);
break;
default:
digitalWrite(laserPin, LOW);
break;
}
}
break;
case 's': // motor speed
break;
case 'z': // reset procedure
// cmd
break;
case 'k':
break;
}
SerialInBufCnt = 0;
}
else {
SerialInBufCnt++; // set pointer to next buffer slot
}
if (SerialInBufCnt > 5) { // valid input if more than 5 were received
SerialInBufCnt = 0; // reset array pointer
Serial.flush(); // clear incoming buffer
}
}
}
// Interrupt on A changing state
void doEncoderA(){
// Test transition
A_set = digitalRead(encoderPinA) == HIGH;
// and adjust counter + if A leads B
encoderPos += (A_set != B_set) ? +1 : -1;
}
// Interrupt on B changing state
void doEncoderB(){
// Test transition
B_set = digitalRead(encoderPinB) == HIGH;
// and adjust counter + if B follows A
encoderPos += (A_set == B_set) ? +1 : -1;
}