void update_pid_outout(void)
{
if(speed_active)
speed_active--;
else
{
leftPID.AimEncoder = 0;
rightPID.AimEncoder = 0;
}
encoder_update();
doPID(&rightPID);
doPID(&leftPID);
set_motor_speeds(leftPID.output, rightPID.output);
}
void PID::run()
{
setCancelState(false); // make sure we don't get cancelled while holding a mutex, etc
timer.reset();
mut.lock();
state.reset();
recomputePeriod(); // calls timer.setPeriod, sets some class member vars
pleaseStop = false;
while (!pleaseStop) {
double e, u = 0;
mut.unlock(); // release the lock
e = cb->getE(); // obtain e from client code without the lock
mut.lock(); // obtain the lock again
// do stuff, compute u based on e and cp params
u = doPID(e, period/static_cast<double>(1e6) /* timestep is in millis */);
recomputePeriod(); // only does something if rate changed..
mut.unlock();
cb->setU(u); // apply 'u' computed above with lock released
timer.waitNextPeriod(); // wait for next period with lock released
mut.lock(); // acquire the lock again because we are checking pleaseStop
}
mut.unlock(); // at this point lock is always held, release it
}
/* Read the encoder values and call the PID routine */
void DiffDrive::updatePID() {
/* Read the encoders */
leftPID.Encoder = MotorControl.queryBrushlessCounter(1);
rightPID.Encoder = MotorControl.queryBrushlessCounter(2);
/* If we're not moving there is nothing more to do */
if (!moving)
return;
/* Compute PID update for each motor */
doPID(&leftPID);
doPID(&rightPID);
/* Set the motor speeds accordingly */
MotorControl.commandMotorPower(1,leftPID.output);
MotorControl.commandMotorPower(2,rightPID.output);
}
int main(void) {
setup();
FILE *fp;
fp = fopen("/dev/servoblaster", "w");
if (fp == NULL) {
printf("Error opening file\n");
exit(0);
}
while(1)
{
doPID(fp);
delay(dt * 1000);
}
return 0;
}
/**
* Performs the Thermostat's current task. Expect to use this during loop().
* This function is called by work().
*
*
* @returns The time taken to run the method
*/
int Ohmbrewer::Thermostat::doWork() {
unsigned long start = micros();
if (getState()){
//enable timer
// _timer->start();
doPID();
}else{
//Shut down procedure
//if thermostat is turned off then turn off element too.
getElement()->setState(false);
getElement()->work();//reset element
//stop Timer
// _timer->stop();
//if PID is off then update temp sensor
getSensor()->work();
}
return micros() - start;
}
/*****************************************************
* Timer 5 is used to provide the main system clock
* It generates an interrupt every millisecond
*
* This ISR is where most of the actual work gets done.
* with the sensors running and the straights profiler
* active, this interrupt takes about 220us of which
* 120us is processing the sensors
*****************************************************/
void _ISR SYSTIM_INTERRUPT(void)
{
int pidOut;
unsigned char rxBytes;
/* reset the interrupt flag */
SYSTIM_IF = 0;
//LED_ON;
tickCount++;
millisecondCount--;
commCount++;
if(!GDO0)
{
rxBytes = CC2500_receive_packet();
if(rxBytes>PACKET_LEN)
{
CC2500_idle_mode();
CC2500_clear_rx_fifo();
CC2500_clear_tx_fifo();
CC2500_receive_mode();
commEnabled = FALSE;
}
else
commEnabled = TRUE;
if(newPacket)
{
deassamble_packet();
getFellowCoveredSqrs();
my.obzClear = OBZ_clear();
if(inOBZ(fellow.location))
{
LED_ON;
}
else
{
LED_OFF;
}
dataUpdated = TRUE;
newPacket = FALSE;
noCommCount = 0;
}
else
noCommCount++;
}
if((commCount>=6)&&commEnabled)
{
assamble_packet();
if(!GDO0)
{
CC2500_transmit_packet();
commCount = 0;
}
}
else if(commCount==4)
{
CC2500_idle_mode();
__delay_us(1);
CC2500_clear_rx_fifo();
__delay_us(1);
CC2500_clear_tx_fifo();
__delay_us(1);
CC2500_receive_mode();
}
if(!(tickCount&1))
readCubeSensors();
if(tickCount>300000L)
{
motorsOff();
sensorsOff();
stopSystemTimer();
LED_ON;
}
doButtons();
readLineSensors();
readCounters();
doProfiler();
pidOut = doPID( &left_PID_param);
if( pidOut < -MOTORS_MAX_DC )
pidOut = -MOTORS_MAX_DC;
else if( pidOut > MOTORS_MAX_DC )
pidOut = MOTORS_MAX_DC;
motorsLeftSetDutyCycle(pidOut);
pidOut = doPID( &right_PID_param);
if( pidOut < -MOTORS_MAX_DC )
pidOut = -MOTORS_MAX_DC;
else if( pidOut > MOTORS_MAX_DC )
pidOut = MOTORS_MAX_DC;
motorsRightSetDutyCycle(pidOut);
//LED_OFF;
}
