void setDefaultPIDConstants(int fd)
{
//Default PID constants
setPIDConstants(fd, 1, 1, -19200, -8000, -150);
setPIDConstants(fd, 2, 1, -19200, -8000, -150);
setPIDConstants(fd, 1, 0, -5248, -75, -500);
setPIDConstants(fd, 2, 0, -10000, -75, -500);
}
int main(int argc, char *argv[])
{
// Prints usage statement
if (argc != 8) {
printf("usage: ./setPID <board number> <motor> <pos_vel> ");
printf("<P> <I> <D> <motor>\n");
printf(" <board number> is [0-3] for placement of cape\n");
printf(" <target> is the target QEI position or velocity\n");
printf(" <pos_vel> 0 for position, 1 for velocity\n");
printf(" <P> is the Kp\n");
printf(" <I> is the Ki\n");
printf(" <D> is the Kd\n");
printf(" <motor> is the motor number\n");
printf("examples: ./setPID 0 5000 0 -5248 -75 -500 1\n");
exit(1);
}
// Get the arguments from the command line
int boardNum = atol(argv[1]);
unsigned int target = atoi(argv[2]);
int indicator = atol(argv[3]);
int P = atol(argv[4]);
int I = atol(argv[5]);
int D = atol(argv[6]);
unsigned int nMotor = atoi(argv[7]);
// Catch Ctrl-C to kill all motors
signal(SIGINT, sig_handler);
// Begin the session (open a connection to the board)
session = DMCCstart(boardNum);
resetQEI(session, nMotor);
// Set the PID Constants
setPIDConstants(session, nMotor, indicator, P, I, D);
// Check if the user has specified target position or velocity
if (indicator == 0) {
moveUntilPos(session, nMotor, target, TIME_LIMIT);
} else if (indicator == 1) {
moveUntilVel(session, nMotor, target, TIME_LIMIT);
} else {
setMotorPower(session, nMotor, 0);
printf("Error: position or velocity not correctly specified\n");
DMCCend(session);
return -1;
}
DMCCend(session);
return 0;
}
int DMCCstart(unsigned char capeAddr)
{
capeAddr+=0x2c;
char filename[20];
strcpy(filename,"/dev/i2c-1");
int fd;
//Opens a file descriptor to the board
fd = open(filename, O_RDWR);
if (fd <0) {
printf("Error: cannot open %s\n",filename);
exit(1);
}
if (ioctl(fd, I2C_SLAVE, capeAddr) < 0) {
printf("Error: cannot ioctl to addr 0x%x\n", capeAddr);
close(fd);
exit(1);
}
int boardVer = getVersionNumber(capeAddr - 0x2c);
int softVer = checkID(fd, boardVer);
if (softVer != 0) {
printf("Error: software version and board version are incompatible\n");
printf(" Board version = %d, Software version = %d\n",
boardVer, softVer);
printf("NOTE: if any version number is -1, the version is not found\n");
close(fd);
exit(1);
}
//Default PID constants
setPIDConstants(fd, 1, -19200, -8000, -150, 1);
setPIDConstants(fd, 1, -19200, -8000, -150, 1);
setPIDConstants(fd, 0, -5248, -75, -500, 1);
setPIDConstants(fd, 0, -10000, -75, -500, 2);
return fd;
}
static PyObject *
dmcc_setPIDConstants(PyObject *self, PyObject *args)
{
unsigned int nBoard;
unsigned int nMotor;
unsigned int posOrVel;
int P;
int I;
int D;
// 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.setPIDConstants takes 6 arguments: board number, motor number, posOrVel, P, I, D
if (!PyArg_ParseTuple(args, "IIIiii:setPIDConstants", &nBoard, &nMotor,
&posOrVel, &P, &I, &D)) {
return NULL;
}
// validate the board number
if (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;
}
if (posOrVel > 1) {
sprintf(szErrorMsg, "posOrVal %d is invalid. posOrVal 0 or 1",
posOrVel);
PyErr_SetString(PyExc_IndexError,szErrorMsg);
return NULL;
}
if ((P > 32767) || (P < -32768)) {
sprintf(szErrorMsg, "P=%d is invalid. P must be between -32768 and 32768",
P);
PyErr_SetString(PyExc_IndexError,szErrorMsg);
return NULL;
}
if ((I > 32767) || (I < -32768)) {
sprintf(szErrorMsg, "I=%d is invalid. I must be between -32768 and 32768",
I);
PyErr_SetString(PyExc_IndexError,szErrorMsg);
return NULL;
}
if ((D > 32767) || (D < -32768)) {
sprintf(szErrorMsg, "D=%d is invalid. D must be between -32768 and 32768",
D);
PyErr_SetString(PyExc_IndexError,szErrorMsg);
return NULL;
}
int session;
session = DMCCstart(nBoard);
setPIDConstants(session, nMotor, posOrVel, P, I, D);
DMCCend(session);
return Py_BuildValue("i", 0);
}
void hardwareInit(){
// Configure the device for maximum performance but do not change the PBDIV
// Given the options, this function will change the flash wait states, RAM
// wait state and enable prefetch cache but will not change the PBDIV.
// The PBDIV value is already set via the pragma FPBDIV option above..
SYSTEMConfig(SYS_FREQ, SYS_CFG_WAIT_STATES | SYS_CFG_PCACHE);
SYSTEMConfigPerformance(80000000);
(_TRISF5)=INPUT; // for the reset sw
ATX_DISENABLE();
CloseTimer2();
Pic32_Bowler_HAL_Init();
Bowler_Init();
clearPrint();
println_I("\n\n\nStarting PIC initialization");
FlashGetMac(MyMAC.v);
DelayMs(2000);//This si to prevent runaway during programming
// enable driven to 3.3v on uart 1
mPORTDOpenDrainClose(BIT_3); mPORTFOpenDrainClose(BIT_5);
char macStr[13];
for (i=0;i<6;i++){
macStr[j++]=GetHighNib(MyMAC.v[i]);
macStr[j++]=GetLowNib(MyMAC.v[i]);
}
macStr[12]=0;
println_I("MAC address is =");
print_I(macStr);
char * dev = "BowlerDevice";
println_I(dev);
//This Method calls INTEnableSystemMultiVectoredInt();
usb_CDC_Serial_Init(dev,macStr,0x04D8,0x0001);
addNamespaceToList((NAMESPACE_LIST *)getBcsCartesianNamespace());
addNamespaceToList((NAMESPACE_LIST *)getBcsPidNamespace());
ATX_ENABLE(); // Turn on ATX Supply, Must be called before talking to the Encoders!!
println_I("Starting Encoders");
initializeEncoders();// Power supply must be turned on first
println_I("Starting Heater");
initializeHeater();
println_I("Starting Servos");
initServos();
#if !defined(NO_PID)
println_I("Starting PID");
initPIDLocal();
#endif
initializeCartesianController();
DelayMs(100);
if( GetPIDCalibrateionState(linkToHWIndex(0))!=CALIBRARTION_DONE&&
GetPIDCalibrateionState(linkToHWIndex(1))!=CALIBRARTION_DONE&&
GetPIDCalibrateionState(linkToHWIndex(2))!=CALIBRARTION_DONE
){
for(i=0;i<numPidMotors;i++){
SetPIDEnabled(i,true) ;
}
println_I("Running calibration for kinematics axis");
runPidHysterisisCalibration(linkToHWIndex(0));
runPidHysterisisCalibration(linkToHWIndex(1));
runPidHysterisisCalibration(linkToHWIndex(2));
DelayMs(100);//wait for ISR to fire and update all values
for(i=0;i<3;i++){
setPIDConstants(linkToHWIndex(i),.2,.1,0);
}
OnPidConfigure(0);
}else{
println_W("Axis are already calibrated");
}
pid.MsTime=getMs();
//startHomingLinks();
disableSerialComs(true) ;
(_TRISB0)=1;
SetColor(1,1,1);
HEATER_2_TRIS = OUTPUT;
//HEATER_1_TRIS = OUTPUT; // Causes one of the axies to crawl downward in bursts when enabled and on...
//HEATER_0_TRIS = OUTPUT; // causes device to twitc. These are touched by the USB stack somehow..... and as the reset button
}
