Using previous values in calculations

Projects General Guidance
1

I'm trying to take gyroscope readings (pitch and roll) and use them to control two sets of servo motors seperately. The problem is that when I switch from controlling set 0 (pitch_pos[0] & roll_pos[0]) to set 1 (pitch_pos[1] & roll_pos[1]), move the sensor and then go back to set 0, those motors will jerk into a different position instantly.

I triend to come up with an equation that would save the last value in each set before switching and using it with together with the current gyroscope angle to keep motors in current set in the same position until the sensor is moved in that set only. I know this should be simple but I can't get my head around it.

//STATE 0 - BASE, ELBOW, WRIST-VERTICAL, GRIPPER OPEN
  if(flex_raw > 540) { //[0] //flex_raw = flex sensor value
    pitch_old[1] = pitch_pos[1];
    roll_old[1] = roll_pos[1];
    pitch_pos[0] = pitch - (pitch_old[0] - pitch_old[1]); //doesn't work
    roll_pos[0] = roll - (roll_old[0] - roll_old[1]);     //doesn't work
    pos[0] = map(roll_pos[0], -75, 75,allowed_min[0], allowed_max[0]);       //M1 (Base)
    pos[2] = map(pitch_pos[0], -75, 75, allowed_min[2], allowed_max[2]); //M3 (Elbow)
    pos[3] = map(pitch_pos[0], -75, 75, allowed_min[3], allowed_max[3]);     //M4 (Wrist - Vertical)
    pos[5] = allowed_min[5];
    //pitch_now[0] = pitch_pos[0];
    //roll_now[0] = roll_pos[0];
  }

  //STATE 1 - SHOULDER, WRIST-ROTATION, GRIPPER OPEN
  if(flex_raw <= 540 && flex_raw > 510) { //[1]
    pitch_old[0] = pitch;
    roll_old[0] = roll;
    pitch_pos[1] = pitch - (pitch_old[1] - pitch_old[0]); //doesn't work
    roll_pos[1] = roll - (roll_old[1] - roll_old[0]);     //doesn't work
    pos[1] = map(pitch_pos[1], -75, 75, allowed_min[1], allowed_max[1]); //M2 (Shoulder)
    pos[4] = map(roll_pos[1], -75, 75, allowed_min[4], allowed_max[4]);  //M5 (Wrist - Rotation)
    pos[5] = allowed_min[5];
    //pitch_now[1] = pitch_pos[1];
    //roll_now[1] = roll_pos[1];

  }

  //STATE 2 - BASE, ELBOW, WRIST-VERTICAL, GRIPPER CLOSED
  if(flex_raw <= 510 && flex_raw > 480) { //[2]
    pitch_old[1] = pitch_pos[1];
    roll_old[1] = roll_pos[1];
    pitch_pos[0] = pitch - (pitch_old[0] - pitch_old[1]); //doesn't work
    roll_pos[0] = roll - (roll_old[0] - roll_old[1]);     //doesn't work
    pos[0] = map(roll_pos[0], -75, 75,allowed_min[0], allowed_max[0]);       //M1 (Base)
    pos[2] = map(pitch_pos[0], -75, 75, allowed_min[2], allowed_max[2]); //M3 (Elbow)
    pos[3] = map(pitch_pos[0], -75, 75, allowed_min[3], allowed_max[3]);     //M4 (Wrist - Vertical)
    pos[5] = allowed_max[5];
    //pitch_now[0] = pitch_pos[0];
    //roll_now[0] = roll_pos[0];
  }

  //STATE 3 - SHOULDER, WRIST-ROTATION, GRIPPER CLOSED
  if(flex_raw <= 480) { //[3]
    pitch_old[0] = pitch_pos[0];
    roll_old[0] = roll_pos[0];
    pitch_pos[1] = pitch - (pitch_old[1] - pitch_old[0]); //doesn't work
    roll_pos[1] = roll - (roll_old[1] - roll_old[0]);     //doesn't work
    pos[1] = map(pitch_pos[1], -75, 75, allowed_min[1], allowed_max[1]); //M2 (Shoulder)
    pos[4] = map(roll_pos[1], -75, 75, allowed_min[4], allowed_max[4]);  //M5 (Wrist - Rotation)
    pos[5] = allowed_max[5];
    //pitch_now[1] = pitch_pos[1];
    //roll_now[1] = roll_pos[1]; 
  }
2

Try and presnt the first paragraph using real data. "Jerk into different positions" is too vague.

3

What I mean is that after switching from set 0 to set 1, the servos will instantly jump to a different position because the values for pitch and roll coming from the sensor will be different compared to what they were before the previous switch. I want to save the pitch_pos[0] , roll_pos[0], pitch_pos[1] & roll_pos[1] and make them independent even though they are controlled by the same pitch and roll varaibles.

For example, I start the system and keep the sensor flat so all values are 0.
I'm in state 0 so when I rotate the sensor, the motors M1, M3 and M4 will follow it.
Then I switch to state 1 which controls M2 and M5 while the sensor is in the rotated position so the M2 and M5 try to reach that position also.
In this scenario I want M2 and M5 to not move until I change the position of the sensor while I'm in state 1, same goes the other way.

Sorry if I'm explaining poorly, I've spent couple hours on this and my brain is fried

4

Come back tomorrow when your brain is a little fresher and able to explain the problem clearly. Right now, you are frustrated and not in the right frame of mind to explain clearly what is going on and what you want the project to do. The giveaway is the expression "doesn't work".

Hopefully in the morning you will realise that to have any chance of helping you, the forum needs to have a much clearer description of what your project is about, what components you are using, links to the specs or data sheets of those components, a schematic (or at least some kind of wiring diagram), and your full code. Some pictures might also help to give others an idea of what you are working on.

5

What is pitch, pitch_pos[0], pitch_pos[1], pitch_old[0] and pitch_old[1] ? Why do you need all these five values?
If you want to control trend, isn't you need only pitch and pitch_old?

6

pitch is the value coming from the sensor, pitch_pos[0] is pitch value for set 0 of motors after calculations, pitch_pos[0] is pitch value for set 0 of motors after calculations, pitch_old[0] is for saving the pitch_pos[0] value during a state switch, similarly for the pitch_pos[1] and pitch_old[1].

I don't know what I need honestly. I really want to give up but can't

7

I agree with the sentiment that you should walk away from this tonight (if it is night where you are), get some sleep or beer or whatever you need to refresh yourself. Tomorrow the problem will look a lot different.

The way you describe your project concerns me because you should separate reading inputs, be they gyros or anything else, from controlling the outputs. Your input data should be continuously updated. If you are not doing that then your code needs modifying so it does. You probably need to learn how to write non blocking, multi tasking code, from your description I don't think that's what you have.

8

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.