void loop()
{
input = encoderRead();
if(new_kP != old_kP)
{
myPID.SetTunings(new_kP,0,0);
old_kP = new_kP;
}
myPID.Compute();
double motorInput;
if (output < -1)
{
motorInput = (112.0/127.0)*output -15;
}
else if (output > 1)
{
motorInput = (112.0/127.0)*output +15;
}
else
{
motorInput = output;
}
motorInput = motorInput+127.0;
mySerial.write((int)motorInput);
delay(120);
}
int main()
{
Network network;
Accelerometer imu;
imu.bypassDrift();
motors.setToZero();
signal(SIGABRT, sigHandler);
signal(SIGINT, sigHandler);
signal(SIGKILL, sigHandler);
signal(SIGQUIT, sigHandler);
signal(SIGTERM, sigHandler);
float ypr[3];
while(true) {
if(imu.getFIFOCount() > 42) {
imu.getYawPitchRoll(ypr);
float p_computed = p_pid.compute(ypr[1], p_target), r_computed = r_pid.compute(ypr[2], r_target);
motors.setSpeed(MOTOR_FL, throttle + r_computed - p_computed);
motors.setSpeed(MOTOR_BL, throttle + r_computed + p_computed);
motors.setSpeed(MOTOR_FR, throttle - r_computed - p_computed);
motors.setSpeed(MOTOR_BR, throttle - r_computed + p_computed);
network.send(SET_MEASURED_VALUES, ypr, sizeof(float)*3, false);
}
}
exit(EXIT_SUCCESS);
}
void setup() {
// put your setup code here, to run once:
pinMode(pinHeater, OUTPUT);
pinMode(pinThermistor, INPUT);
digitalWrite(pinHeater, 0);
Serial.begin(57600);
Serial.setTimeout(250); // Set timeout to 250ms
// Restore calibration if already set.
thermCalibrated = true;
if(EEPROM.read(thermStepSetAddr) == 0) {
unsigned char setPoint[4];
for(int i = 0; i < 4; i++)
setPoint[i] = EEPROM.read(thermStepAddr+i);
thermStep = *((float*)setPoint);
} else {
thermCalibrated = false;
}
if(EEPROM.read(thermYIntSetAddr) == 0) {
unsigned char setPoint[4];
for(int i = 0; i < 4; i++)
setPoint[i] = EEPROM.read(thermYIntAddr+i);
thermYInt = *((float*)setPoint);
} else {
thermCalibrated = false;
}
pid.SetOutputLimits(0.0f, 1.0f);
pid.SetSampleTime(250); // 250ms compute period
pid.SetMode(MANUAL);
}
void computePID(void) {
if((pitch_set < PITCH_MIN) || (pitch_set > PITCH_MAX)) pitch_set = pitch_set_last;
if((roll_set < ROLL_MIN) || (roll_set > ROLL_MAX)) roll_set = roll_set_last;
if((yaw_set < YAW_MIN) || (yaw_set > YAW_MAX)) yaw_set = yaw_set_last;
pitch_set_last = pitch_set;
roll_set_last = roll_set;
pitch_set_last = pitch_set;
//////////////////////////////////////////////////////
//////////////////////////////////////////////////////
// FOR TEST ONLY /////////////////////////////////////
//////////////////////////////////////////////////////
//////////////////////////////////////////////////////
ypr[0] = 0;
if(abs(ypr[0]-yprLast[0])>30) ypr[0] = yprLast[0];
if(abs(ypr[1]-yprLast[1])>30) ypr[1] = yprLast[1];
if(abs(ypr[2]-yprLast[2])>30) ypr[2] = yprLast[2];
yprLast[0] = ypr[0];
yprLast[1] = ypr[1];
yprLast[2] = ypr[2];
pitchReg.Compute();
rollReg.Compute();
yawReg.Compute();
//trace_printf("yprPID: %f, %f, %f\n", bal_yaw, bal_pitch, bal_roll);
}
bool GMEventManager::AddNewGMEvent (Client* client, csString eventName, csString eventDescription)
{
int newEventID, zero=0;
PID gmID = client->GetPID();
int clientnum = client->GetClientNum();
GMEvent* gmEvent;
// if this GM already has an active event, he/she cant start another
if ((gmEvent = GetGMEventByGM(gmID, RUNNING, zero)) != NULL)
{
psserver->SendSystemInfo(clientnum,
"You are already running the \'%s\' event.",
gmEvent->eventName.GetDataSafe());
return false;
}
if (eventName.Length() > MAX_EVENT_NAME_LENGTH)
{
eventName.Truncate(MAX_EVENT_NAME_LENGTH);
psserver->SendSystemInfo(client->GetClientNum(),
"Event name truncated to \'%s\'.",
eventName.GetDataSafe());
}
// GM can register his/her event
newEventID = GetNextEventID();
// remove undesirable characters from the name & description
csString escEventName, escEventDescription;
db->Escape(escEventName, eventName.GetDataSafe());
db->Escape(escEventDescription, eventDescription.GetDataSafe());
// first in the database
db->Command("INSERT INTO gm_events(id, gm_id, name, description, status) VALUES (%i, %i, '%s', '%s', %i)",
newEventID,
gmID.Unbox(),
escEventName.GetDataSafe(),
escEventDescription.GetDataSafe(),
RUNNING);
// and in the local cache.
gmEvent = new GMEvent;
gmEvent->id = newEventID;
gmEvent->gmID = gmID;
gmEvent->EndTime = 0;
gmEvent->eventName = eventName;
gmEvent->eventDescription = eventDescription;
gmEvent->status = RUNNING;
gmEvents.Push(gmEvent);
// keep psCharacter upto date
client->GetActor()->GetCharacterData()->AssignGMEvent(gmEvent->id,true);
psserver->SendSystemInfo(client->GetClientNum(),
"You have initiated a new event, \'%s\'.",
eventName.GetDataSafe());
return true;
}
int main(){
/* Test water tank input output
* output should decrease */
double level = 0;
double vin = 0;
double vout = 0;
double ref = 500;
int period = 100;
PIDParameters pp(1,2,1,0,0.1,period,ref,0,255,false);
PID pid;
pid.updateParameters(pp);
std::cout << pp << "\n";
for (int i = 0; i < 7000; i++){
vin = 0.2* pid.next(level);
pid.updateStates();
vout = 0.1 * sqrt(2*9.82*level);
level = level + (vin - vout) * period/1000;
if (level < 0) level = 0;
std::cout << ref - level << "\n";
}
}
void PIDControl::optionsPID(int setTimePoint)
{
//tell the PID to range between 0 and the full window size
myPID.SetOutputLimits(0, setTimePoint);
//turn the PID on
myPID.SetMode(AUTOMATIC);
}
//Sets up the internal state monitor (PID for heating and Watchdog)
void internalStateSetup() {
//TMP102.h temperature ranges from -40 to 125 C
controller.setInputLimits(-40.0, 125.0);
controller.setOutputLimits(0.0, 1.0);
controller.setBias(0.3); // Try experimenting with the bias!
controller.setMode(AUTO_MODE);
controller.setSetPoint(DESIRED_INTERNAL_TEMP);
W.Start(WATCH_DOG_RATE);
}
void CharCreationManager::HandleCharDelete(MsgEntry* me, Client* client)
{
psCharDeleteMessage msg(me);
csString charName = msg.charName;
if(!charName.Length())
{
CPrintf(CON_WARNING,"Client %u sent malformed character name to deletion code!",client->GetClientNum());
return; // No char
}
PID pid = psserver->CharacterLoader.FindCharacterID(client->GetAccountID(), charName);
csString error;
if(psserver->CharacterLoader.AccountOwner(charName, client->GetAccountID()))
{
// Found the char?
if(!pid.IsValid())
{
psserver->SendSystemError(client->GetClientNum(),"Couldn't find character data!");
return;
}
// Can we delete it?
if(!psserver->CharacterLoader.DeleteCharacterData(pid, error))
{
psserver->SendSystemError(client->GetClientNum(),"Error: %s",error.GetData());
return;
}
// Remove cached objects to make sure that the client gets a fresh character
// list from the database.
iCachedObject* obj = psserver->GetCacheManager()->RemoveFromCache(psserver->GetCacheManager()->MakeCacheName("list",client->GetAccountID().Unbox()));
if(obj)
{
obj->ProcessCacheTimeout();
obj->DeleteSelf();
}
obj = psserver->GetCacheManager()->RemoveFromCache(psserver->GetCacheManager()->MakeCacheName("auth", client->GetAccountID().Unbox()));
if(obj)
{
obj->ProcessCacheTimeout();
obj->DeleteSelf();
}
// Ok, we deleted it
psCharDeleteMessage response(charName, me->clientnum);
response.SendMessage();
}
else
{
CPrintf(CON_WARNING,"Character %s not deleted because of non-ownership\n", charName.GetData());
psserver->SendSystemError(client->GetClientNum(),"You do not own that character!");
return;
}
}
void handle_enabled() {
if (settings.enabled) {
reset_window();
pid.SetMode(AUTOMATIC);
} else {
cancel_autotune();
pid.SetMode(MANUAL);
set_heat(false);
}
}
//---------- Functions -------------------------------
void init_pids ()
{
right_mtr_pid.SetSampleTime(10);
right_mtr_pid.SetMode(AUTOMATIC);
//right_mtr_pid.SetOutputLimits(20,240); //min, max
left_mtr_pid.SetSampleTime(10);
left_mtr_pid.SetMode(AUTOMATIC);
//left_mtr_pid.SetOutputLimits(20,240); //min, max
}
void Stop(PID& pid) {
pid.SetMode(MANUAL);
SetSpeed(0);
m_fPower = 0.0;
m_nTicksPID = 0;
m_nLastCompute = 0;
analogWrite(POWER, 0);
pid.SetMode(AUTOMATIC);
}
msg_t velocity_node(void *arg) {
r2p::Node node("velocity");
r2p::Subscriber<r2p::Velocity3Msg, 5> vel_sub;
r2p::Velocity3Msg *velp;
r2p::Subscriber<r2p::QEIMsg, 5> qei1_sub(qei1_callback);
r2p::Subscriber<r2p::QEIMsg, 5> qei2_sub(qei2_callback);
r2p::Publisher<r2p::Velocity3Msg> odometry_pub;
float v;
float w;
(void) arg;
chRegSetThreadName("velocity");
node.subscribe(qei1_sub, "qei1");
node.subscribe(qei2_sub, "qei2");
node.subscribe(vel_sub, "velocity");
if (!node.advertise(odometry_pub, "odometry")) while(1);
vel_pid.config(1.0, 3, 0, 0.05, -5.0, 5.0);
// vel_pid.config(2.0, 2, -0.1, 0.05, -5.0, 5.0);
// vel_pid.config(5.0, 1.0, 0.00, 0.05, -5.0, 5.0);
// vel_pid.config(5.0, 1.0, 0.05, 0.05, -5.0, 5.0);
vel_pid.set(0);
for (;;) {
if (!node.spin(r2p::Time::ms(500))) {
angle_setpoint = 0;
continue;
}
v = (left_speed + right_speed) / 2;
w = (right_speed - left_speed) / 0.425;
angle_setpoint = vel_pid.update(v); // 20hz
r2p::Velocity3Msg *msgp;
if (odometry_pub.alloc(msgp)) {
msgp->x = v;
msgp->y = 0;
msgp->w = w;
odometry_pub.publish(*msgp);
}
while (vel_sub.fetch(velp)) {
palTogglePad(LED2_GPIO, LED2);
vel_setpoint = velp->x;
w_setpoint = velp->w;
vel_sub.release(*velp);
}
vel_pid.set(vel_setpoint);
}
return CH_SUCCESS;
}
/*Each time this is called, this pets the watchdog and computes how much the heater must be powered in order to maintain
the desired temperature. If you every want to change the internal workings of the system, this is the function where you put that
code*/
void internalStateLoop(const void *context) {
//Pet the watchdog
W.Pet();
controller.setProcessValue(internal_temp); //We won't actually read from the TMP 102.h, we'll use the most recent internal temp variable (global).
// Set the new output.
heater = controller.compute();
printf("What should the output be? %f\n", controller.compute());
// Now check for termination conditions
// 1. If the GPS lat,lon exceed the permitted bounds, cut down.
// 2. If you receive an iridum command telling you to end the flight, cut down.
// 3. If you've not received an Iridium command in a while (5 hrs), cut down.
}
void motorsInit( int leftPin, int rightPin )
{
servoLeft.attach(leftPin);
servoRight.attach(rightPin);
servoLeft.writeMicroseconds(servoSpeedLeft);
servoRight.writeMicroseconds(servoSpeedRight);
#ifdef USE_PID
Input = 0;
Setpoint = 0;
turningPID.SetMode(AUTOMATIC);
turningPID.SetOutputLimits(-SERVO_DRIVE_TURN_SPEED, SERVO_DRIVE_TURN_SPEED);
#endif
}
/*
* Balance control node
*/
msg_t balance_node(void *arg) {
r2p::Node node("balance");
r2p::Subscriber<r2p::TiltMsg, 2> tilt_sub;
r2p::TiltMsg *tiltp;
r2p::Publisher<r2p::PWM2Msg> pwm2_pub;
r2p::PWM2Msg *pwmp;
int32_t pwm = 0;
(void) arg;
chRegSetThreadName("balance");
node.advertise(pwm2_pub, "pwm2");
node.subscribe(tilt_sub, "tilt");
PID pid;
//pid.config(800, 0.35, 0.09, 0.02, -4000, 4000);
pid.config(800, 0.35, 0.01, 0.02, -4000, 4000);
//pid.config(550, 0.6, 0, 0.02, -4000, 4000);
//pid.config(500, 990.35, 0, 0.02, -4000, 4000);
// pid.config(600, 0.35, 0, 0.02, -4000, 4000);
//
//
//
pid.set(angle_setpoint);
for (;;) {
pid.set(angle_setpoint);
while (!tilt_sub.fetch(tiltp)) {
r2p::Thread::sleep(r2p::Time::ms(1));
}
pwm = pid.update(tiltp->angle); //rad2grad
tilt_sub.release(*tiltp);
if (pwm2_pub.alloc(pwmp)) {
pwmp->value[0] = (pwm - w_setpoint * 100); //FIXME
pwmp->value[1] = -(pwm + w_setpoint * 100); //FIXME
pwm2_pub.publish(*pwmp);
palTogglePad(LED3_GPIO, LED3);
palSetPad(LED4_GPIO, LED4);
} else {
palClearPad(LED4_GPIO, LED4);
}
}
return CH_SUCCESS;
}
void loop() {
// 22,0,0 is lowest value for sustaining oscillation
// 1.48,0,0 is highest value for almost no reverse
static float P = 1.48, I = 0, D = 0;
static long setpoint = 0;
if (Serial.available()) { // User started typing something
if (Serial.peek() == 'p') {
Serial.read();
Serial.println(F("\r\nGive a P value"));
scanf_P(PSTR("%f"), &P);
Serial.println(P);
} else if (Serial.peek() == 'i') {
Serial.read();
Serial.println(F("\r\nGive a I value"));
scanf_P(PSTR("%f"), &I);
Serial.println(I);
} else if (Serial.peek() == 'd') {
Serial.read();
Serial.println(F("\r\nGive a D value"));
scanf_P(PSTR("%f"), &D);
Serial.println(D);
} else if (Serial.peek() == 's') {
Serial.read();
Serial.println(F("\r\nGive a Setpoint"));
scanf_P(PSTR("%ld"), &setpoint);
Serial.println(setpoint);
} else {
Serial.println(F("\r\nEnter new PID values"));
scanf_P(PSTR("%f %f %f"), &P, &I, &D);
printf_P(PSTR("%f %f %f"), P, I, D);
}
}
botPID.SetPID(P, I, D);
bot.UpdateEncoders();
long encdists[4];
bot.GetDists(encdists);
long xdist, ydist;
bot.GetPos(&xdist, &ydist);
int wheelspd = botPID.GetOutput(xdist, setpoint, micros());
bot.Run(0, wheelspd);
//printf_P(PSTR(/*"P:%.2f I:%.2f D:%.2f "*/"curdist:%4ld setpoint:%4ld curspd:%4d\n"),
// P, I, D, xdist, setpoint, wheelspd);
delay(10);
}
void setup(void)
{
board.init();
// sleep for 100ms
board.delayMilliseconds(100);
// flash the LEDs to indicate startup
board.ledRedOn();
board.ledGreenOff();
for (uint8_t i = 0; i < 10; i++) {
board.ledRedToggle();
board.ledGreenToggle();
board.delayMilliseconds(50);
}
board.ledRedOff();
board.ledGreenOff();
// initialize our RC, IMU, mixer, and PID controller
rc.init(&board);
imu.init(&board);
pid.init();
mixer.init(&board, &rc, &pid);
msp.init(&board, &imu, &mixer, &rc);
// set initial time
previousTime = board.getMicros();
// always do gyro calibration at startup
calibratingG = CONFIG_CALIBRATING_GYRO_CYCLES;
// trigger accelerometer calibration requirement
haveSmallAngle = true;
} // setup
PID EntityManager::CopyNPCFromDatabase(PID master_id, float x, float y, float z, float angle, const csString & sector, InstanceID instance, const char *firstName, const char *lastName)
{
psCharacter * npc = NULL;
PID new_id;
npc = psServer::CharacterLoader.LoadCharacterData(master_id, false);
if (npc == NULL)
{
return 0;
}
psSectorInfo* sectorInfo = CacheManager::GetSingleton().GetSectorInfoByName( sector );
if (sectorInfo != NULL)
{
npc->SetLocationInWorld(instance,sectorInfo,x,y,z,angle);
}
if ( firstName && lastName )
{
npc->SetFullName( firstName, lastName );
}
if (psServer::CharacterLoader.NewNPCCharacterData(0, npc))
{
new_id = npc->GetPID();
db->Command("UPDATE characters SET npc_master_id=%d WHERE id=%d", master_id.Unbox(), new_id.Unbox());
}
delete npc;
return new_id;
}
void loop(void) {
SetTemp=cfg.DispTemp;
SetHumi=cfg.DispHumi;
float ndo = read_ds(&ds_stt);
if(ndo!=9999) NowTemp=ndo;
read_dht(&dht_temp,&dht_humi);
double gap = abs(SetTemp-NowTemp); //distance away from setpoint
if (gap < 10){ //we're close to setpoint, use conservative tuning parameters
myPID.SetTunings(consKp, consKi, consKd);
}else{ //we're far from setpoint, use aggressive tuning parameters
myPID.SetTunings(aggKp, aggKi, aggKd);
}
myPID.Compute();
analogWrite(SSR_PIN, (byte)OutPWM);
if(dht_humi<SetHumi){ digitalWrite(HUMI_GENERATE, HIGH);}else{ digitalWrite(HUMI_GENERATE, LOW);}
Display();
}
void onSetAngleOutputLimits()
{
Serial.println("Receiving Angle output limits command...");
float lMin, lMax;
readLimitsArgs(&lMin, &lMax);
anglePID.SetOutputLimits(lMin, lMax);
cmdMessenger.sendCmd(kStatus, "Angle output limits set");
}
void onSetAnglePID()
{
Serial.println("Receiving Angle tuning command...");
float Kp, Ki, Kd;
readTuningArgs(&Kp, &Ki, &Kd);
anglePID.SetTunings(Kp, Ki, Kd);
cmdMessenger.sendCmd(kStatus, "Angle PID tuned");
}
bool MotorControl::set_angular_rate(float angular_rate) {
if (angular_rate < 0 || angular_rate > 1) return false;
angular_rate_ = angular_rate;
acceleration_rate_ = 1 - angular_rate;
pid->setOutputLimits(-angular_rate_, angular_rate_);
return true;
}
void setup()
{
//initialize the variables we're linked to
Input = analogRead(0);
Setpoint = 100;
//turn the PID on
myPID.SetMode(AUTO);
}
void loop()
{
hal.scheduler->delay(20);
//rc.read_pwm();
long error = hal.rcin->read(0) - radio_trim;
long control= pid.get_pid(error, 1);
hal.console->print("control: ");
hal.console->println(control,BASE_DEC);
}
float dutyCycleBasedOnProportionalControl(float currentTemperature)
{
static PID pid;
static float temperatureTarget = 0.0f;
float previousTemperatureTarget = temperatureTarget;
temperatureTarget = currentScript.currentTemperatureTarget();
if (ISNAN(temperatureTarget)) {
writeFailure("Temperature setpoint is NAN. Are thermocouples connected?");
return 0.0f;
}
if (previousTemperatureTarget != temperatureTarget) {
PID newPID(temperatureTarget, P_GAIN, 0, 0, PID_OUTPUT_LIMIT);
pid = newPID;
}
return pid.nextControlOutput(currentTemperature, 0);
}
void setup()
{
Wire.begin(myAddress);
Wire.onReceive(i2cReceive);
Wire.onRequest(i2cRequest);
pinMode(limit_switch_pin, INPUT);
mySerial.begin(9600); // Serial commuication to motor controller start.
setpoint = 0.0;
calibration(); // Running the calibration code on every start up
input = encoderRead();
myPID.SetMode(AUTOMATIC);
myPID.SetOutputLimits(-127, 127);
myPID.SetSampleTime(20);
}
void control_loop() {
pinMode( control_pin, OUTPUT );
if(tuning && auto_tune.Runtime()) {
finish_autotune();
} else if(!tuning) {
pid.SetMode(!config.paused);
pid.SetTunings(profiles[config.driving].kp, profiles[config.driving].ki, profiles[config.driving].kd);
pid.SetSampleTime(1000 * profiles[config.driving].sample_time); // Update the control value once per second
current_target_temp = profiles[config.driving].target_temp;
pid.Compute();
}
if(last_power != power) {
last_power = power;
analogWrite(control_pin, power);
Serial.print("Log\tpower\t");
Serial.println((100.0 * ((float)power / 255.0)));
}
}
bool ComputePID(PID& pid) {
unsigned long nNow = millis();
unsigned long nChange = nNow - m_nLastCompute;
m_fTicksPerSecond = m_nTicksPID / (nChange / 1000.0);
if(pid.Compute()) { // is the sample time low enough?
m_nTicksPID = 0;
m_nLastCompute = nNow;
analogWrite(POWER, (int)m_fPower);
return true;
}
return false;
}
msg_t pid_node(void * arg) {
pid_conf * conf = reinterpret_cast<pid_conf *>(arg);
r2p::Node node("pid");
r2p::Subscriber<r2p::SpeedMsg, 5> speed_sub;
r2p::Subscriber<r2p::EncoderMsg, 5> enc_sub(enc_callback);
r2p::SpeedMsg * msgp;
r2p::Time last_setpoint(0);
(void) arg;
chRegSetThreadName("pid");
speed_pid.config(conf->k, conf->ti, conf->td, conf->ts, -4095.0, 4095.0);
/* Enable the h-bridge. */
palSetPad(GPIOB, GPIOB_MOTOR_ENABLE); palClearPad(GPIOA, GPIOA_MOTOR_D1);
chThdSleepMilliseconds(500);
pwmStart(&PWM_DRIVER, &pwmcfg);
node.subscribe(speed_sub, "speed");
node.subscribe(enc_sub, "encoder");
for (;;) {
if (node.spin(r2p::Time::ms(1000))) {
if (speed_sub.fetch(msgp)) {
speed_pid.set(msgp->value);
last_setpoint = r2p::Time::now();
speed_sub.release(*msgp);
} else if (r2p::Time::now() - last_setpoint > r2p::Time::ms(1000)) {
speed_pid.set(0);
}
} else {
// Stop motor if no messages for 1000 ms
pwm_lld_disable_channel(&PWM_DRIVER, 0);
pwm_lld_disable_channel(&PWM_DRIVER, 1);
}
}
return CH_SUCCESS;
}
