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; }