0

I'm trying to get error_x and error_y to display the difference between the target and the position. For some reason I cannot get error_x to function correctly. error_y has the same code, but works correctly. Can anyone help?

Here is the code on my Arduino:

void setup() {
  Serial.begin(9600);
  Serial.print("Starting...\n");
}

int previousTime = 0;
float lastError_x = 0;
float lastError_y = 0;

void loop() {
  //pid variables
  float Kp = 1;
  float Ki = 0;
  float Kd = 0;
  float pos_x;
  float pos_y;
  float target_x;
  float target_y;
  float error_x;
  float error_y;
  int currentTime;
  int elapsedTime;
  float cumError_x;
  float cumError_y;
  float rateError_x;
  float rateError_y;
  float output_x;
  float output_y;

  //invkinematic variables
  float inVec[4] = {1, -1.5, 3};
  float height = 3.5;
  float B = 4;
  float L = 3;
  float U = 3.5;
  float plateRadius = 5.5;
  float alpha[4] = {0, 0, 0};
  float normVec[4];
  float p1[4] = {0,0,0};
  float p2[4] = {0,0,0};
  float p3[4] = {0,0,0};
  float crosskv[4] = {0,0,0};
  float normSave;
  float dotSave;
  float radius;
  float gamma;
  float beta;

  //serial variables
  char buf[32];
  char str_temp1[6];
  char str_temp2[6];
  char str_temp3[6];


  //PIXY CAMERA CODE (outputs x and y)

  pos_x = 1;
  pos_y = 1;

  //DETERMINE TARGET LOCATION
  //default:(0,0)
  target_x = 0;
  target_y = 0;

  //PID CODE (inputs x and y and outputs normal vector and height)

  error_x = target_x - pos_x;
  error_y = target_y - pos_y;

  dtostrf(error_x, 5, 3, str_temp1);
  dtostrf(error_y, 5, 3, str_temp2);
  sprintf(buf, "error1:%s error2:%s\n", str_temp1, str_temp2);
  Serial.print(buf);

  currentTime = millis();
  elapsedTime = currentTime - previousTime;
  if(elapsedTime==0){elapsedTime = 1;}

  cumError_x += error_x * elapsedTime / 1000;
  cumError_y += error_y * elapsedTime / 1000;
  rateError_x = (error_x - lastError_x)/(elapsedTime/1000);
  rateError_y = (error_y - lastError_y)/(elapsedTime/1000);

  output_x = Kp*error_x + Ki*cumError_x + Kd * rateError_x;
  output_y = Kp*error_y + Ki*cumError_y + Kd * rateError_y;

  inVec[0] = output_x/plateRadius;
  inVec[1] = output_y/plateRadius;
  inVec[2] = plateRadius;

  lastError_x = error_x;
  lastError_y = error_y;
  previousTime = currentTime;

  //INVKINEMATIC (inputs normal vector and height and outputs alpha values)

  normVec[0] = inVec[0]/(sqrt(inVec[0]*inVec[0]+inVec[1]*inVec[1]+inVec[2]*inVec[2]));
  normVec[1] = inVec[1]/(sqrt(inVec[0]*inVec[0]+inVec[1]*inVec[1]+inVec[2]*inVec[2]));
  normVec[2] = inVec[2]/(sqrt(inVec[0]*inVec[0]+inVec[1]*inVec[1]+inVec[2]*inVec[2]));

  p1[0] = normVec[2];
  p1[2] = -1*normVec[0];
  normSave = sqrt(p1[0]*p1[0]+p1[1]*p1[1]+p1[2]*p1[2]);
  p1[0] = plateRadius*p1[0]/normSave;
  p1[1] = plateRadius*p1[1]/normSave;
  p1[2] = plateRadius*p1[2]/normSave;

  crosskv[0] = normVec[1]*p1[2] - normVec[2]*p1[1];
  crosskv[1] = normVec[2]*p1[0] - normVec[0]*p1[2];
  crosskv[2] = normVec[0]*p1[1] - normVec[1]*p1[0];
  dotSave = normVec[0]*p1[0] + normVec[1]*p1[1] + normVec[2]*p1[2];
  p2[0] = -0.5*p1[0] + crosskv[0]*0.866 + normVec[0]*dotSave*1.5;
  p2[1] = -0.5*p1[1] + crosskv[1]*0.866 + normVec[1]*dotSave*1.5;
  p2[2] = -0.5*p1[2] + crosskv[2]*0.866 + normVec[2]*dotSave*1.5;
  normSave = sqrt(p2[0]*p2[0]+p2[1]*p2[1]+p2[2]*p2[2]);
  p2[0] = plateRadius*p2[0]/normSave;
  p2[1] = plateRadius*p2[1]/normSave;
  p2[2] = plateRadius*p2[2]/normSave;

  p3[0] = -0.5*p1[0] + crosskv[0]*-0.866 + normVec[0]*dotSave*1.5;
  p3[1] = -0.5*p1[1] + crosskv[1]*-0.866 + normVec[1]*dotSave*1.5;
  p3[2] = -0.5*p1[2] + crosskv[2]*-0.866 + normVec[2]*dotSave*1.5;
  normSave = sqrt(p3[0]*p3[0]+p3[1]*p3[1]+p3[2]*p3[2]);
  p3[0] = plateRadius*p3[0]/normSave;
  p3[1] = plateRadius*p3[1]/normSave;
  p3[2] = plateRadius*p3[2]/normSave;

  p1[2] = height+p1[2];
  p2[2] = height+p2[2];
  p3[2] = height+p3[2];

  radius = sqrt((p1[0]-B)*(p1[0]-B)+p1[1]*p1[1]+p1[2]*p1[2]);
  gamma = atan2(p1[2],sqrt((p1[0]-B)*(p1[0]-B)+(p1[1])*(p1[1])));
  beta = acos((L*L+radius*radius-U*U)/(2*L*radius));
  alpha[0] = 3.1416 - (gamma + beta);

  radius = sqrt((p2[0]+0.5*B)*(p2[0]+0.5*B)+(p2[1]-0.866*B)*(p2[1]-0.866*B)+(p2[2])*(p2[2]));
  gamma = atan2(p2[2],sqrt((p2[0]+0.5*B)*(p2[0]+0.5*B)+(p2[1]-0.866*B)*(p2[1]-0.866*B)));
  beta = acos((L*L+radius*radius-U*U)/(2*L*radius));
  alpha[1] = 3.1416 - (gamma + beta);

  radius = sqrt((p3[0]+0.5*B)*(p3[0]+0.5*B)+(p3[1]+0.866*B)*(p3[1]+0.866*B)+(p3[2])*(p3[2]));
  gamma = atan2(p3[2],sqrt((p3[0]+0.5*B)*(p3[0]+0.5*B)+(p3[1]+0.866*B)*(p3[1]+0.866*B)));
  beta = acos((L*L+radius*radius-U*U)/(2*L*radius));
  alpha[2] = 3.1416 - (gamma + beta);

  dtostrf(normVec[0], 5, 3, str_temp1);
  dtostrf(normVec[1], 5, 3, str_temp2);
  dtostrf(normVec[2], 5, 3, str_temp3);
  sprintf(buf, "a1:%s a2:%s a3:%s\n", str_temp1, str_temp2, str_temp3);
  Serial.print(buf);

  //OUTPUT alpha to servos

  delay(1000);
}

Here is the output with the error:

Starting...
error1: error2:-1.000
a1:  NAN a2:  NAN a3:

error1: error2:-1.000
a1: a2:-0.033 a3:

error1: error2:-1.000
a1: a2:-0.033 a3:

error1: error2:-1.000
a1: a2:-0.033 a3:

error1: error2:-1.000
a1: a2:-0.033 a3:
  • Try to print the errors directly with multiple Serial.print() statements, ditching all the dtostrf() and sprintf(). – chrisl Nov 9 at 22:23
0

Consider enlarging your character arrays. When creating strings an extra character is likely necessary to indicate the end of a string. This is usually the null character or zero. If missing, many string related features will not see an end of a string and likely cause unexpected results.

  • What I think happened, the 1st dtostrf() call created a properly formed string. however, the null character likely ended up at the beginning of the next character array. Which likely got over written by the 2nd dtostrf() call. – st2000 Nov 9 at 23:49
  • I'm actually getting a whole new error. Something seems very wrong now. – Wes Summers Nov 10 at 0:36
  • The serial output is now: Sta⸮⸮⸮⸮⸮⸮⸮⸮⸮ – Wes Summers Nov 10 at 0:36
  • So the 1st print in setup() is not working? Likely something else is wrong. You have a huge number of floats for an embedded processor project. This requires a lot of memory. And most of the floats are local variable so are on the stack. That means memory is set aside for them as you run your program. So running out of memory is not easily check at compile time. You might be running out of memory. This depends on the actual stack usage and the specific processor your Arduino platform is using. Try cutting back on the number of floats variables. – st2000 Nov 10 at 14:29
  • Also, I appreciate getting the answer marked as correct. But you may want to remove that until it is understood why the program does not work so that others do not skip over your question. Debugging a program in a stack exchange Q/A never works well as the question usually changes over time making a mess of the Q/A. In future, simplify your program to the minimum which still exhibits the unexpected results then post your question. – st2000 Nov 10 at 14:53

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.