int main(int argc, char *argv[])
{
// Prints usage statement
if (argc != 2) {
printf("usage: ./getQEI <board number>\n");
printf(" <board number> is [0-3] for placement of cape\n");
printf("example: ./getQEI 0\n");
exit(1);
}
int boardNum = atol(argv[1]);
int session = DMCCstart(boardNum);
unsigned int motor1QEI, motor2QEI;
int vel1, vel2;
while (1) {
// Read the Quadrature Encoder Interface (QEI) for motor 1 and 2
motor1QEI = getQEI(session, 1);
motor2QEI = getQEI(session, 2);
vel1 = getQEIVel(session, 1);
vel2 = getQEIVel(session, 2);
printf("QEI Motor 1 = %u (0x%x) [v = %d], Motor 2 = %u (0x%x) [v = %d]\n",
motor1QEI, motor1QEI, vel1, motor2QEI, motor2QEI, vel2);
// Wait 0.2 seconds before next reading
usleep(200000);
}
DMCCend(session);
return 0;
}
static PyObject *
dmcc_getQEI(PyObject *self, PyObject *args)
{
int nBoard;
int nMotor;
// Make sure szErrorMsg is not on the stack
// -- downside is that we could have concurrency issues with different
// threads, but you know what, there should only be one error message
// at a time. If you have it from multiple threads, your code is fubar'ed
// anyways!
static char szErrorMsg[80];
// DMCC.getQEI takes 2 arguments: board number, motor number
if (!PyArg_ParseTuple(args, "ii:getQEI", &nBoard, &nMotor)) {
return NULL;
}
// validate the board number
if ((nBoard < 0) || (nBoard > 3)) {
sprintf(szErrorMsg, "Board number %d is invalid. Board number must be between 0 and 3.",
nBoard);
PyErr_SetString(PyExc_IndexError,szErrorMsg);
return NULL;
}
// validate the motor number
if ((nMotor < 1) || (nMotor > 2)) {
sprintf(szErrorMsg, "Motor number %d is invalid. Motor number must be 1 or 2.",
nMotor);
PyErr_SetString(PyExc_IndexError,szErrorMsg);
return NULL;
}
int session;
unsigned int nQEI;
session = DMCCstart(nBoard);
nQEI = getQEI(session, nMotor);
DMCCend(session);
return Py_BuildValue("i", nQEI);
}
int main(){
uint8_t start_r, old_IPL;
uint8_t hz50_scaler, hz5_scaler, hz1_scaler, sec;
uint32_t tick = 0;
hz50_scaler = hz5_scaler = hz1_scaler = sec = 0;
touch_init();
__delay_ms(200);
lcd_initialize(); // Initialize the LCD
motor_init();
lcd_clear();
lcd_locate(0,0);
lcd_printf("-- Ball position: --");
timers_initialize(); // Initialize timers
while (1) {
start_r = 0;
while(!start_r) {
// disable all maskable interrupts
SET_AND_SAVE_CPU_IPL(old_IPL, 7);
start_r = start;
// enable all maskable interrupts
RESTORE_CPU_IPL(old_IPL);
}
// Periodic real-time task code starts here = CENTER_X + RADIUS * cos(tick * SPEED);
// Ypos_set = CENT!!!
double pidX, pidY;
uint16_t duty_us_x, duty_us_y;
// 50Hz control task
if(hz50_scaler == 0) {
calcQEI(Xpos_set, Xpos, Ypos_set, Ypos);
// Xpos_set = CENTER_X;
// Xpos_set = CENTER_Y;
Xpos_set = CENTER_X + RADIUS * cos(tick * SPEED);
Ypos_set = CENTER_Y + RADIUS * sin(tick * SPEED);
tick++;
pidX = pidX_controller(Xpos);
pidY = pidY_controller(Ypos);
// TODO: Convert PID to motor duty cycle (900-2100 us)
// setMotorDuty is a wrapper function that calls your motor_set_duty
// implementation in flexmotor.c. The 2nd parameter expects a value
// between 900-2100 us
// duty_us_x = cap((pidX*1000.0), 2100, 900);
// duty_us_y = cap((pidY*1000.0), 2100, 900);
duty_us_x = cap((pidX + 1500), 2100, 900);
duty_us_y = cap((pidY + 1500), 2100, 900);
motor_set_duty(1, duty_us_x);
motor_set_duty(2, duty_us_y+100);
// setMotorDuty(MOTOR_X_CHAN, duty_us_x);
// setMotorDuty(MOTOR_Y_CHAN, duty_us_y);
}
// 5Hz display task
if(hz5_scaler == 0) {
// lcd_locate(0,1);
// lcd_printf("Xp=%.1f,Yp=%.1f", Xpos, Ypos);
// lcd_locate(0,2);
// lcd_printf("X*=%.1f, Y*=%.1f", Xpos_set, Ypos_set);
// lcd_locate(0,3);
// lcd_printf("pX=%.1f,pY=%.1f", pidX, pidY);
// lcd_locate(0,4);
// lcd_printf("dx=%u, dY=%u", duty_us_x, duty_us_y);
//
if(deadline_miss >= 1) {
lcd_locate(0,6);
lcd_printf("%4d d_misses!!!", deadline_miss);
}
}
// 1Hz seconds display task
if(hz1_scaler == 0) {
lcd_locate(0,7);
lcd_printf("QEI: %5u", getQEI());
sec++;
}
hz50_scaler = (hz50_scaler + 1) % 2;
hz5_scaler = (hz5_scaler + 1) % 20;
hz1_scaler = (hz1_scaler + 1) % 100;
start = 0;
}
return 0;
}
