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); }
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 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"); }
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))); } }
void update_pid_tunings() { pid.SetTunings(settings.pid.kp, settings.pid.ki, settings.pid.kd); }
void loop() { // put your main code here, to run repeatedly: String str = Serial.readStringUntil('\n'); const char *cmd = str.c_str(); if(strncmp(cmd, "AT+ SetTemp", 11) == 0 && strlen(cmd) > 13) { const char *target = cmd + 12; targetTemp = (float)atoi(target) / 100.0f; // Don't allow 100C to be exceeded. if(targetTemp > 100.0f) targetTemp = 100.0f; // Convert to Kelvin targetTemp += 273.15; // PID SetMode method ignores if we go from automatic // to automatic pid.SetMode(AUTOMATIC); Serial.print("AT- SetTempOk\r\n"); } else if(strncmp(cmd, "AT+ GetActualTemp", 17) == 0) { char buf[64]; snprintf(buf, 64, "AT- ActualTemp %u\r\n", (unsigned int)((actualTemp-273.15) * 100)); Serial.print(buf); } else if(strncmp(cmd, "AT+ GetTargetTemp", 17) == 0) { char buf[64]; snprintf(buf, 64, "AT- TargetTemp %u\r\n", (unsigned int)((targetTemp-273.15) * 100)); Serial.print(buf); } else if(strncmp(cmd, "AT+ TurnOff", 11) == 0) { pid.SetMode(MANUAL); pulseWidth = 0; Serial.print("AT- TurnOffOk\r\n"); } else if(str.length() > 0) { Serial.print("AT- UnknownCmd\r\n"); } // Get actual temp from thermistor // Using Steinhart-Hart. Based on // http://playground.arduino.cc/ComponentLib/Thermistor2 int pinValue = analogRead(pinThermistor); //float v1 = (float)pinValue / 1024.0f * supplyVoltage; // Simple voltage divider. // v1 = supplyVoltage * Rt / (balanceResistor + Rt) // Solve for Rt. float rVal = balanceResistor * (1023.0f/pinValue-1); float lnR = log(rVal); float tinv = tA + tB * lnR + tC * lnR * lnR * lnR; actualTemp = 1.0f / tinv; if(fabs(actualTemp - targetTemp) > 5) { pid.SetTunings(kPfar, kIfar, kDfar); } else { pid.SetTunings(kPnear, kInear, kDnear); } pid.Compute(); // TODO: not sure if analogWrite will work correctly, // may turn heater pad on/off too quickly or relay may be // too slow. analogWrite(pinHeater, pulseWidth * 255); }
int main(int argc, char **argv) { START_EASYLOGGINGPP(argc, argv); // Load configuration from file el::Configurations conf("/home/debian/hackerboat/embedded_software/unified/setup/log.conf"); // Actually reconfigure all loggers instead el::Loggers::reconfigureAllLoggers(conf); BoatState *me = new BoatState(); me->rudder = new Servo(); me->throttle = new Throttle(); me->orient = new OrientationInput(SensorOrientation::SENSOR_AXIS_Z_UP); double targetHeading = 0; double in = 0, out = 0, setpoint = 0; Pin enable(Conf::get()->servoEnbPort(), Conf::get()->servoEnbPin(), true, true); PID *helm = new PID(&in, &out, &setpoint, 0, 0, 0, 0); helm->SetMode(AUTOMATIC); helm->SetControllerDirection(Conf::get()->rudderDir()); helm->SetSampleTime(Conf::get()->rudderPeriod()); helm->SetOutputLimits(Conf::get()->rudderMin(), Conf::get()->rudderMax()); helm->SetTunings(10,0,0); if (!me->rudder->attach(Conf::get()->rudderPort(), Conf::get()->rudderPin())) { std::cout << "Rudder failed to attach 1" << std::endl; return -1; } if (!me->rudder->isAttached()) { std::cout << "Rudder failed to attach 2" << std::endl; return -1; } if (me->orient->begin() && me->orient->isValid()) { cout << "Initialization successful" << endl; cout << "Oriented with Z axis up" << endl; } else { cout << "Initialization failed; exiting" << endl; return -1; } for (int i = 0; i < 100; i++) { double currentheading = me->orient->getOrientation()->makeTrue().heading; if (isfinite(currentheading)) targetHeading += currentheading; cout << "."; std::this_thread::sleep_for(100ms); } cout << endl; targetHeading = targetHeading/100; cout << "Target heading is " << to_string(targetHeading) << " degrees true " << endl; int count = 0; for (;;) { in = me->orient->getOrientation()->makeTrue().headingError(targetHeading); count++; LOG_EVERY_N(10, DEBUG) << "True Heading: " << me->orient->getOrientation()->makeTrue() << ", Target Course: " << targetHeading; helm->Compute(); me->rudder->write(out); LOG_EVERY_N(10, DEBUG) << "Rudder command: " << to_string(out); std::this_thread::sleep_for(100ms); if (count > 9) { count = 0; cout << "True Heading: " << me->orient->getOrientation()->makeTrue().heading << "\tTarget Course: " << targetHeading << "\tRudder command: " << to_string(out) << endl; } } return 0; }