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);
}
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);
}
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;
}
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 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)));
}
}
double PIDControl::reflowPID(double setTempPoint, double setTimePoint, unsigned long windowStartTime)
{
_val=analogRead(sensePin);
currTemp = _val * 0.00489 / 0.005; //Converting the voltage of sensePin to current temp.
displayTemp(currTemp,setTempPoint);
myPID.Compute();
Serial.println(Output);
return Output;
_now = millis();
if((_now - windowStartTime)/1000 > setTimePoint)
{ //time to shift the Reflow Window
windowStartTime += setTimePoint;
}
if (Output > (_now - windowStartTime)/1000)
{
digitalWrite(heaterPin,HIGH);
}
else digitalWrite(heaterPin,LOW);
}
void update() {
if (settings.enabled) {
if (autotune_running) {
if (pid_autotune.Runtime()) {
autotune_running = false;
settings.pid.kp = pid_autotune.GetKp();
settings.pid.ki = pid_autotune.GetKi();
settings.pid.kd = pid_autotune.GetKd();
update_pid_tunings();
write_settings();
}
}
if (! autotune_running) {
pid.Compute();
}
pid_output_sum += pid_output;
pid_output_count++;
unsigned long now = millis();
if (now >= window_end_millis) {
pid_output_average = pid_output_sum / pid_output_count;
if (pid_output_average >= settings.heat_on.threshold) {
set_heat(true);
} else if (pid_output_average + settings.heat_on.delta <=
settings.heat_on.threshold) {
set_heat(false);
}
reset_window();
}
}
}
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);
}
/* Calcule les pwm a appliquer pour un asservissement en vitesse en trapeze
* <> value_pwm_left : la pwm a appliquer sur la roue gauche [-255,255]
* <> value_pwm_right : la pwm a appliquer sur la roue droite [-255,255]
* */
void speedControl(int* value_pwm_left, int* value_pwm_right){
/* si le robot est en train de tourner, et qu'on lui donne une consigne de vitesse, il ne va pas partir droit
* solution = decomposer l'asservissement en vitesse en 2 :
* -> stopper le robot (les 2 vitesses = 0)
* -> lancer l'asservissement en vitesse
*/
static int start_time;
static bool initDone = false;
if(!initDone){
start_time = 0;
pwm = 0;
currentSpeed = 0;
consigne = 0;
pid4SpeedControl.Reset();
pid4SpeedControl.SetInputLimits(-20000,20000);
pid4SpeedControl.SetOutputLimits(-255,255);
pid4SpeedControl.SetSampleTime(DUREE_CYCLE);
pid4SpeedControl.SetMode(AUTO);
initDone = true;
}
/* Gestion de l'arret d'urgence */
if(current_goal.isCanceled){
initDone = false;
current_goal.isReached = true;
current_goal.isCanceled = false;
/* et juste pour etre sur */
(*value_pwm_right) = 0;
(*value_pwm_left) = 0;
return;
}
/* Gestion de la pause */
if(current_goal.isPaused){
(*value_pwm_right) = 0;
(*value_pwm_left) = 0;
return;
}
if(current_goal.phase == PHASE_1){ //phase d'acceleration
consigne = current_goal.speed;
currentSpeed = robot_state.speed;
if(abs(consigne-currentSpeed) < 1000){ /*si l'erreur est inferieur a 1, on concidere la consigne atteinte*/
current_goal.phase = PHASE_2;
start_time = millis();
}
}
else if(current_goal.phase == PHASE_2){ //phase de regime permanent
consigne = current_goal.speed;
currentSpeed = robot_state.speed;
if(millis()-start_time > current_goal.period){ /* fin de regime permanent */
current_goal.phase = PHASE_3;
}
}
else if(current_goal.phase == PHASE_3){ //phase de decceleration
consigne = 0;
currentSpeed = robot_state.speed;
if(abs(robot_state.speed)<1000){
current_goal.phase = PHASE_4;
}
}
pid4SpeedControl.Compute();
if(current_goal.phase == PHASE_4){
(*value_pwm_right) = 0;
(*value_pwm_left) = 0;
current_goal.isReached = true;
initDone = false;
}else{
(*value_pwm_right) = pwm;
(*value_pwm_left) = pwm;
}
}
/* Calcule les pwm a appliquer pour un asservissement en position
* <> value_pwm_left : la pwm a appliquer sur la roue gauche [-255,255]
* <> value_pwm_right : la pwm a appliquer sur la roue droite [-255,255]
* */
void positionControl(int* value_pwm_left, int* value_pwm_right){
static bool initDone = false;
if(!initDone){
output4Delta = 0;
output4Alpha = 0;
currentDelta = .0;
currentAlpha = .0;
consigneDelta = .0;
consigneAlpha = .0;
pid4DeltaControl.Reset();
pid4DeltaControl.SetInputLimits(-TABLE_DISTANCE_MAX_MM/ENC_TICKS_TO_MM,TABLE_DISTANCE_MAX_MM/ENC_TICKS_TO_MM);
pid4DeltaControl.SetSampleTime(DUREE_CYCLE);
pid4DeltaControl.SetOutputLimits(-current_goal.speed,current_goal.speed); /*composante liee a la vitesse lineaire*/
pid4DeltaControl.SetMode(AUTO);
pid4AlphaControl.Reset();
pid4AlphaControl.SetSampleTime(DUREE_CYCLE);
pid4AlphaControl.SetInputLimits(-M_PI,M_PI);
pid4AlphaControl.SetOutputLimits(-255,255); /*composante lie a la vitesse de rotation*/
pid4AlphaControl.SetMode(AUTO);
initDone = true;
}
/* Gestion de l'arret d'urgence */
if(current_goal.isCanceled){
initDone = false;
current_goal.isReached = true;
current_goal.isCanceled = false;
/* et juste pour etre sur */
(*value_pwm_right) = 0;
(*value_pwm_left) = 0;
return;
}
/* Gestion de la pause */
if(current_goal.isPaused){
(*value_pwm_right) = 0;
(*value_pwm_left) = 0;
return;
}
/*calcul de l'angle alpha a combler avant d'etre aligne avec le point cible
* borne = [-PI PI] */
double angularCoeff = atan2(current_goal.y-robot_state.y,current_goal.x-robot_state.x); /*arctan(y/x) -> [-PI,PI]*/
currentAlpha = moduloPI(angularCoeff - robot_state.angle); /* il faut un modulo ici, c'est sur !
/* En fait, le sens est donne par l'ecart entre le coeff angulaire et l'angle courant du robot.
* Si cet angle est inferieur a PI/2 en valeur absolue, le robot avance en marche avant (il avance quoi)
* Si cet angle est superieur a PI/2 en valeur absolue, le robot recule en marche arriere (= il recule)
*/
int sens = 1;
if(abs(currentAlpha) > M_PI/2){/* c'est a dire qu'on a meilleur temps de partir en marche arriere */
sens = -1;
currentAlpha = moduloPI(M_PI + angularCoeff - robot_state.angle);
}
currentAlpha = -currentAlpha;
double dx = current_goal.x-robot_state.x;
double dy = current_goal.y-robot_state.y;
currentDelta = -sens * sqrt(dx*dx+dy*dy); // - parce que l'ecart doit etre negatif pour partir en avant
/*
Serial.print("coeff:");
Serial.print(angularCoeff);
Serial.print(" angle:");
Serial.print(robot_state.angle);
Serial.print(" alpha:");
Serial.print(currentAlpha);
Serial.print(" delta:");
Serial.print(currentDelta);
Serial.print(" x:");
Serial.print(current_goal.x);
Serial.print(" y:");
Serial.println(current_goal.y);
*/
/* on limite la vitesse lineaire quand on s'approche du but */
if (abs(currentDelta)<250){
pid4DeltaControl.SetOutputLimits(-min(50,current_goal.speed),min(50,current_goal.speed)); // composante liee a la vitesse lineaire
pid4AlphaControl.SetOutputLimits(-150,150); // composante liee a la vitesse de rotation
}
else if (abs(currentDelta)<500){
pid4DeltaControl.SetOutputLimits(-min(60,current_goal.speed),min(60,current_goal.speed)); // composante liee a la vitesse lineaire
pid4AlphaControl.SetOutputLimits(-150,150); // composante liee a la vitesse de rotation
}
else if (abs(currentDelta)<750){
pid4DeltaControl.SetOutputLimits(-min(80,current_goal.speed),min(80,current_goal.speed)); // composante liee a la vitesse lineaire
pid4AlphaControl.SetOutputLimits(-150,150); // composante liee a la vitesse de rotation
}
else if (abs(currentDelta)<1000){
pid4DeltaControl.SetOutputLimits(-min(100,current_goal.speed),min(100,current_goal.speed)); // composante liee a la vitesse lineaire
pid4AlphaControl.SetOutputLimits(-150,150); // composante liee a la vitesse de rotation
}
else if (abs(currentDelta)<1250){
pid4DeltaControl.SetOutputLimits(-min(150,current_goal.speed),min(150,current_goal.speed)); // composante liee a la vitesse lineaire
pid4AlphaControl.SetOutputLimits(-150,150); // composante liee a la vitesse de rotation
}
else if (abs(currentDelta)<1500){
pid4DeltaControl.SetOutputLimits(-min(200,current_goal.speed),min(200,current_goal.speed)); // composante liee a la vitesse lineaire
pid4AlphaControl.SetOutputLimits(-150,150); // composante liee a la vitesse de rotation
}
if(abs(currentDelta) < 5*ENC_MM_TO_TICKS) /*si l'ecart n'est plus que de 6 mm, on considere la consigne comme atteinte*/
current_goal.phase = PHASE_2;
else
current_goal.phase = PHASE_1;
pid4AlphaControl.Compute();
pid4DeltaControl.Compute();
if(current_goal.phase == PHASE_2){
(*value_pwm_right) = 0;
(*value_pwm_left) = 0;
}
else{
double pwm4Delta = 0.0;
//FIXME probleme de debordemment
//alpha 200 delta 255 / delta+alpha = 455 / delta-alpha = 55 / => alpha 200 delta 55
//alpha 200 delta -255 / delta+alpha = -55 / delta-alpha = -455 / => alpha 200 delta -55
//alpha -200 delta 255 / delta+alpha = 55 / delta-alpha = 455 / => alpha -200 delta 55
//alpha -200 delta -255 / delta+alpha = -455 / delta-alpha = -55 / => alpha -200 delta -55
/*
Correction by thomas
if(output4Delta+output4Alpha>255 || output4Delta-output4Alpha>255)
pwm4Delta = 255-output4Alpha;
else if(output4Delta+output4Alpha<-255 || output4Delta-output4Alpha<-255)
pwm4Delta = -255+output4Alpha;
else
pwm4Delta = output4Delta;*/
(*value_pwm_right) = output4Delta+output4Alpha;
(*value_pwm_left) = output4Delta-output4Alpha;
// Correction by thomas
if ((*value_pwm_right) > 255)
(*value_pwm_right) = 255;
else if ((*value_pwm_right) < -255)
(*value_pwm_right) = -255;
if ((*value_pwm_left) > 255)
(*value_pwm_left) = 255;
else if ((*value_pwm_left) < -255)
(*value_pwm_left) = -255;
}
if(current_goal.phase == PHASE_2){
if(current_goal.id != -1 && !current_goal.isMessageSent){
//le message d'arrivee n'a pas encore ete envoye a l'intelligence
//envoi du message
sendMessage(current_goal.id,2);
current_goal.isMessageSent = true;
}
/*condition d'arret = si on a atteint le but et qu'un nouveau but attends dans la fifo*/
if(!fifoIsEmpty()){ //on passe a la tache suivante
current_goal.isReached = true;
initDone = false;
}
}
}
/* Calcule les pwm a appliquer pour un asservissement en angle
* <> value_pwm_left : la pwm a appliquer sur la roue gauche [-255,255]
* <> value_pwm_right : la pwm a appliquer sur la roue droite [-255,255]
* */
void angleControl(int* value_pwm_left, int* value_pwm_right){
static bool initDone = false;
if(!initDone){
pwm = 0;
currentEcart = .0;
consigne = .0;
pid4AngleControl.Reset();
pid4AngleControl.SetInputLimits(-M_PI,M_PI);
pid4AngleControl.SetOutputLimits(-current_goal.speed,current_goal.speed);
pid4AngleControl.SetSampleTime(DUREE_CYCLE);
pid4AngleControl.SetMode(AUTO);
initDone = true;
}
/* Gestion de l'arret d'urgence */
if(current_goal.isCanceled){
initDone = false;
current_goal.isReached = true;
current_goal.isCanceled = false;
/* et juste pour etre sur */
(*value_pwm_right) = 0;
(*value_pwm_left) = 0;
return;
}
/* Gestion de la pause */
if(current_goal.isPaused){
(*value_pwm_right) = 0;
(*value_pwm_left) = 0;
return;
}
/*l'angle consigne doit etre comprise entre ]-PI, PI]
En fait pour simplifier, l'entree du PID sera l'ecart entre le l'angle courant et l'angle cible (consigne - angle courant)
la consigne (SetPoint) du PID sera 0
la sortie du PID sera le double pwm
*/
currentEcart = -moduloPI(current_goal.angle - robot_state.angle);
/*
Serial.print("goal: ");
Serial.print(current_goal.angle);
Serial.print(" current: ");
Serial.print(robot_state.angle);
Serial.print(" ecart: ");
Serial.println(currentEcart);
*/
if(abs(currentEcart) < 3.0f*M_PI/360.0f) /*si l'erreur est inferieur a 3deg, on concidere la consigne atteinte*/
current_goal.phase = PHASE_2;
else
current_goal.phase = PHASE_1;
pid4AngleControl.Compute();
if(current_goal.phase == PHASE_2){
(*value_pwm_right) = 0;
(*value_pwm_left) = 0;
}
else{
(*value_pwm_right) = pwm;
(*value_pwm_left) = -pwm;
}
if(current_goal.phase == PHASE_2){
if(current_goal.id != -1 && !current_goal.isMessageSent){
//le message d'arrivee n'a pas encore ete envoye a l'intelligence
//envoi du message
sendMessage(current_goal.id,2);
current_goal.isMessageSent = true;
}
if(!fifoIsEmpty()){ //on passe a la tache suivante
/*condition d'arret = si on a atteint le but et qu'un nouveau but attends dans la fifo*/
current_goal.isReached = true;
initDone = false;
}
}
}
/* Calcule les pwm a appliquer pour un asservissement en position
* <> value_pwm_left : la pwm a appliquer sur la roue gauche [-255,255]
* <> value_pwm_right : la pwm a appliquer sur la roue droite [-255,255]
* */
void positionControl(int* value_pwm_left, int* value_pwm_right){
static bool initDone = false;
if(!initDone){
output4Delta = 0;
output4Alpha = 0;
currentDelta = .0;
currentAlpha = .0;
consigneDelta = .0;
consigneAlpha = .0;
pid4DeltaControl.Reset();
pid4DeltaControl.SetInputLimits(-TABLE_DISTANCE_MAX_MM/ENC_TICKS_TO_MM,TABLE_DISTANCE_MAX_MM/ENC_TICKS_TO_MM);
pid4DeltaControl.SetSampleTime(DUREE_CYCLE);
pid4DeltaControl.SetOutputLimits(-current_goal.speed,current_goal.speed); /*composante liee a la vitesse lineaire*/
pid4DeltaControl.SetMode(AUTO);
pid4AlphaControl.Reset();
pid4AlphaControl.SetSampleTime(DUREE_CYCLE);
pid4AlphaControl.SetInputLimits(-M_PI,M_PI);
pid4AlphaControl.SetOutputLimits(-255,255); /*composante lie a la vitesse de rotation*/
pid4AlphaControl.SetMode(AUTO);
initDone = true;
}
/* Gestion de l'arret d'urgence */
if(current_goal.isCanceled){
initDone = false;
current_goal.isReached = true;
current_goal.isCanceled = false;
/* et juste pour etre sur */
(*value_pwm_right) = 0;
(*value_pwm_left) = 0;
return;
}
/* Gestion de la pause */
if(current_goal.isPaused){
(*value_pwm_right) = 0;
(*value_pwm_left) = 0;
return;
}
/*calcul de l'angle alpha a combler avant d'etre aligne avec le point cible
* borne = [-PI PI] */
double angularCoeff = atan2(current_goal.y-robot_state.y,current_goal.x-robot_state.x); /*arctan(y/x) -> [-PI,PI]*/
currentAlpha = moduloPI(angularCoeff - robot_state.angle); /* il faut un modulo ici, c'est sur !
/* En fait, le sens est donne par l'ecart entre le coeff angulaire et l'angle courant du robot.
* Si cet angle est inferieur a PI/2 en valeur absolue, le robot avance en marche avant (il avance quoi)
* Si cet angle est superieur a PI/2 en valeur absolue, le robot recule en marche arriere (= il recule)
*/
int sens = 1;
if(current_goal.phase != PHASE_1 and abs(currentAlpha) > M_PI/2){/* c'est a dire qu'on a meilleur temps de partir en marche arriere */
sens = -1;
currentAlpha = moduloPI(M_PI + angularCoeff - robot_state.angle);
}
currentAlpha = -currentAlpha;
double dx = current_goal.x-robot_state.x;
double dy = current_goal.y-robot_state.y;
currentDelta = -sens * sqrt(dx*dx+dy*dy); // - parce que l'ecart doit etre negatif pour partir en avant
switch(current_goal.phase)
{
case PHASE_1:
if(abs(currentDelta)<1500) /*si l'ecart n'est plus que de 6 mm, on considere la consigne comme atteinte*/
{
current_goal.phase = PHASE_DECEL;
}
break;
case PHASE_DECEL:
if (fifoIsEmpty())
{
/* on limite la vitesse lineaire quand on s'approche du but */
if (abs(currentDelta)<250){
pid4DeltaControl.SetOutputLimits(-min(50,current_goal.speed),min(50,current_goal.speed)); // composante liee a la vitesse lineaire
pid4AlphaControl.SetOutputLimits(-150,150); // composante liee a la vitesse de rotation
}
else if (abs(currentDelta)<500){
pid4DeltaControl.SetOutputLimits(-min(60,current_goal.speed),min(60,current_goal.speed)); // composante liee a la vitesse lineaire
pid4AlphaControl.SetOutputLimits(-150,150); // composante liee a la vitesse de rotation
}
else if (abs(currentDelta)<750){
pid4DeltaControl.SetOutputLimits(-min(80,current_goal.speed),min(80,current_goal.speed)); // composante liee a la vitesse lineaire
pid4AlphaControl.SetOutputLimits(-150,150); // composante liee a la vitesse de rotation
}
else if (abs(currentDelta)<1000){
pid4DeltaControl.SetOutputLimits(-min(100,current_goal.speed),min(100,current_goal.speed)); // composante liee a la vitesse lineaire
pid4AlphaControl.SetOutputLimits(-150,150); // composante liee a la vitesse de rotation
}
else if (abs(currentDelta)<1250){
pid4DeltaControl.SetOutputLimits(-min(150,current_goal.speed),min(150,current_goal.speed)); // composante liee a la vitesse lineaire
pid4AlphaControl.SetOutputLimits(-150,150); // composante liee a la vitesse de rotation
}
else {
pid4DeltaControl.SetOutputLimits(-min(200,current_goal.speed),min(200,current_goal.speed)); // composante liee a la vitesse lineaire
pid4AlphaControl.SetOutputLimits(-150,150); // composante liee a la vitesse de rotation
}
}
if(abs(currentDelta) < 5*ENC_MM_TO_TICKS) /*si l'ecart n'est plus que de 6 mm, on considere la consigne comme atteinte*/
{
//envoi du message
sendMessage(current_goal.id,2);
current_goal.isMessageSent = true;
current_goal.phase = PHASE_ARRET;
}
break;
case PHASE_ARRET:
if (abs(currentDelta) > 5*ENC_MM_TO_TICKS)
{
current_goal.phase = PHASE_CORRECTION;
}
break;
case PHASE_CORRECTION:
pid4DeltaControl.SetOutputLimits(-min(50,current_goal.speed),min(50,current_goal.speed)); // composante liee a la vitesse lineaire
pid4AlphaControl.SetOutputLimits(-150,150); // composante liee a la vitesse de rotation
if (abs(currentDelta) < 5*ENC_MM_TO_TICKS)
{
current_goal.phase = PHASE_ARRET;
}
default:
break;
}
if (!fifoIsEmpty() and (current_goal.phase == PHASE_ARRET or current_goal.phase == PHASE_CORRECTION)) { //on passe a la tache suivante si la fifo n'est pas vide
current_goal.isReached = true;
initDone = false;
}
pid4AlphaControl.Compute();
pid4DeltaControl.Compute();
if (current_goal.phase == PHASE_ARRET)
{
(*value_pwm_right) = 0;
(*value_pwm_left) = 0;
}
else
{
(*value_pwm_right) = output4Delta+output4Alpha;
(*value_pwm_left) = output4Delta-output4Alpha;
// Débordement
if ((*value_pwm_right) > 255)
(*value_pwm_right) = 255;
else if ((*value_pwm_right) < -255)
(*value_pwm_right) = -255;
if ((*value_pwm_left) > 255)
(*value_pwm_left) = 255;
else if ((*value_pwm_left) < -255)
(*value_pwm_left) = -255;
}
}
void loop()
{
Input = analogRead(0);
myPID.Compute();
analogWrite(3,Output);
}
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;
}
virtual void run() {
int16_t accData[3];
int16_t gyrData[3];
CTimer tm(TIMER0);
tm.second(DT);
tm.enable();
myPID.SetMode(AUTOMATIC);
myPID.SetOutputLimits(-100.0, 100.0);
myPID.SetSampleTime(PID_SAMPLE_RATE);
m_roll = 0.0f;
m_pitch = 0.0f;
double output;
#if USE_AUTO_TUNING
double sp_input, sp_output, sp_setpoint, lastRoll;
PID speedPID(&sp_input, &sp_output, &sp_setpoint, 35, 1, 2, DIRECT);
speedPID.SetMode(AUTOMATIC);
speedPID.SetOutputLimits(-config.roll_offset, config.roll_offset);
speedPID.SetSampleTime(PID_SAMPLE_RATE);
sp_input = 0;
sp_setpoint = 0;
lastRoll = 0;
#endif
//
// loop
//
while (isAlive()) {
//
// wait timer interrupt (DT)
//
if (tm.wait()) {
//
// read sensors
//
m_mxMPU.lock();
m_mpu->getMotion6(&accData[0], &accData[1], &accData[2],
&gyrData[0], &gyrData[1], &gyrData[2]);
m_mxMPU.unlock();
//
// filter
//
ComplementaryFilter(accData, gyrData, &m_pitch, &m_roll);
#if USE_AUTO_TUNING
sp_input = (lastRoll - m_roll) / PID_SAMPLE_RATE; // roll speed
lastRoll = m_roll;
speedPID.Compute();
m_setpoint = -sp_output; // tune output angle
#else
m_roll -= config.roll_offset;
#endif
m_input = m_roll;
myPID.Compute();
if ( m_output > config.skip_interval ) {
LEDs[1] = LED_ON;
LEDs[2] = LED_OFF;
output = map(m_output, config.skip_interval, 100, config.pwm_min, config.pwm_max);
m_left->direct(DIR_FORWARD);
m_right->direct(DIR_FORWARD);
} else if ( m_output<-config.skip_interval ) {
LEDs[1] = LED_OFF;
LEDs[2] = LED_ON;
output = map(m_output, -config.skip_interval, -100, config.pwm_min, config.pwm_max);
m_left->direct(DIR_REVERSE);
m_right->direct(DIR_REVERSE);
} else {
LEDs[1] = LED_OFF;
LEDs[2] = LED_OFF;
output = 0;
m_left->direct(DIR_STOP);
m_right->direct(DIR_STOP);
}
//
// auto power off when fell.
//
if ( abs(m_roll)>65 ) {
output = 0;
}
//
// output
//
m_left->dutyCycle(output * config.left_power);
m_right->dutyCycle(output * config.right_power);
}
}
