Hi,
I am using the Pololu qik 2s12v10 controller with Pololu 37D 50:1 and 100:1 motors and an Arduino MEGA 2560. I am using this library to read and accumulate encoder counts. This library uses interrupts to count. When I am not sending commands to the qik, the encoder output reports the correct value (verified visually as well as a hall-effect rotary position sensor). When I add the qik.setM0Speed(127);
line to my code (in the main loop), the encoder count and angle is incorrect.
Any suggestions to what I could do to fix this? Is the qik library interfering with interrupts?
Thanks,
Aedhan
Code for reference:
//Includes
#include <math.h>
#include <Encoder.h>
#include <SoftwareSerial.h>
#include <PololuQik.h>
//Constants
const int interruptPin = 3;
//Flags
bool flagButton = 0;
//Variable Definitions
float ti = 0; //Inital time (s)
float tf = 1; //Final time (s)
float qi = 0; //Inital pos (deg)
float qf = 90; //Final pos (deg)
float vi = 2; //Inital vel (deg/s)
float vf = 2; //Final vel (deg/s)
float ai = 0; //Inital accl (deg/s^2)
float af = 0; //Final accl (deg/s^2)
float encAngle = 0;
float n = 0.0375; //encoder counts per revolution 9600. 360/9600 = 0.0375
int tilt_angle = 0;
int tilt_angle_last = 0;
int offset = 0;
int tiltPin = A0;
int hallAngle = 0;
int buttonPin = 12;
int rotationCt = 0; //number of rotations
int s = 0; //step count
int s_total;
unsigned long t = 0; //Current time (ms)
unsigned long t_last = 0; //Last loop time (ms)
unsigned long t_start = 0;
int t_step = 100; //Time step (ms)
unsigned long time;
float coeffArray [6] = {1, 1, 2, 3, 4, 5};
float qArray [4] = {0, 0, 0, 0}; //time, position, velocty, acceleration
float qArray_last [4] = {0, 0, 0, 0};
float* qArrayPointer = &qArray[0];
float* coeffPointer = &coeffArray[0];
bool buttonState = 0; //Reset angle button
float setAngle = 0;
float Kp = 3;
float Ki = .05;
float angleError = 0;
float angleErrorLast = 0;
int motorspeed;
float errorSum = 0;
int errorZeroCount = 0;
PololuQik2s12v10 qik(15, 14, 4);
// 15 - RX
// 14 - TX
// 04 - RESET
Encoder myEnc(18, 19);
//------------------------------------------------------
//SETUP
//------------------------------------------------------
void setup() {
// Initalize inputs
pinMode(buttonPin, INPUT);
// Initalize procedures
Serial.begin(115200);
qik.init();
attachInterrupt(digitalPinToInterrupt(interruptPin), ISR1, RISING);
angleReset();
s_total = (tf * 1000 - ti * 1000) / t_step;
t = 0;
t_start = millis();
}
//------------------------------------------------------
//MAIN
//------------------------------------------------------
void loop() {
//Collect inputs -------------------------------------------------------------------------------------------------
//encAngle = (long) myEnc.read()*n*10000; //*10000 to keep as int but maintain accuracy
encAngle = myEnc.read();
tilt_angle = analogRead(tiltPin);
//buttonState = digitalRead(buttonPin);
//Process ---------------------------------------------------------------------------------------------------------
//Calibrate and Map
//Hall effect:
tilt_angle -= 99;
if (flagButton == 1) angleReset();
tilt_angle -= offset;
if (tilt_angle < 0) tilt_angle += 821;
hallAngle = map(tilt_angle, 0, 821, 0, 360);
//Encoder:
encAngle = encAngle * n;
//accululate rotations
if (tilt_angle_last > 350 && tilt_angle < 10) {
rotationCt += 1;
}
if (tilt_angle_last < 10 && tilt_angle > 350) {
rotationCt -= 1;
}
hallAngle = hallAngle + rotationCt * 360;
//Angle Error Calculation using encoder
angleErrorLast = angleError;
angleError = setAngle - encAngle;
errorSum = errorSum + angleErrorLast;
//Output ---------------------------------------------------------------------------------------------------------------
t = millis();
qik.setM0Speed(127);
Serial.println();
//Incrementals ---------------------------------------------------------------------------------------------------------
tilt_angle_last = tilt_angle;
}
//------------------------------------------------------
//FUNCTIONS
//------------------------------------------------------
void move (float q, int motor) {
float Kp = 1;
float Ki = 1;
}
void ISR1() {
Serial.println("HERE");
flagButton = 1;
}
void angleReset(){
tilt_angle = analogRead(tiltPin);
tilt_angle -= 99;
offset = tilt_angle;
Serial.println(offset);
Serial.println(flagButton);
myEnc.write(0);
flagButton = 0;
}