949 Autonomous Code

1. 949 Autonomous Code Jim Wright

2. 949 Code • Encoders • Gyro • PID • State Machine

3. Map • Primitive Encoder/Gyro • Dark Age Encoder (Quadrature) • Dark Age Gyro (Compass(ish)) • Renaissance Encoder • Renaissances Gyro • Enlightenment

4. Primitive Encoder/Gyro • Encoder • Hook a line to the interrupt pin and do the counting that way. • Give you the speed • You need to know which way your going. • Gyro • Hook the gyro to the A/D line and read it.

5. Map • Primitive Encoder/Gyro • Dark Age Encoder (Quadrature) • Dark Age Gyro (Compass(ish)) • Renaissance Encoder • Renaissances Gyro • Enlightenment

6. Quadrature Encoder Wave Form • Phase A and B are shifted by 90 degrees (or 45 degrees or 180 degrees)

7. Connecting the Encoders • // Quadrature Decoder Input Bits (PORTB) • #define RIGHT_A 0x10 • #define RIGHT_B 0x20 • #define LEFT_B 0x40 • #define LEFT_A 0x80 • The right A is connected to Digital I/O 3, The Left A is connected to Digital I/O 6 • These correspond to the Port B interrupt lines

8. The decoding Code • void DecodeQuadrature(unsigned char new) • { • unsigned char x; • x = new ^ old; • x |= (x>>1 & 0x55) | (x<<1 & 0xAA); • x &= ((new>>1 ^ old) & 0x55) | ((new<<1 ^ old) & 0xAA); • old = new; • // Decode (see hardware.h) • if (x & LEFT_A) LeftEncoder--; • if (x & LEFT_B) LeftEncoder++; • if (x & RIGHT_A) RightEncoder--; /* NB Wheels are mirror images */ • if (x & RIGHT_B) RightEncoder++; /* Hence invert (or swap A&B) one encoder */ • } • This is a cut and paste function.

9. Map • Primitive Encoder/Gyro • Dark Age Encoder (Quadrature) • Dark Age Gyro (Compass(ish)) • Renaissance Encoder • Renaissances Gyro • Enlightenment

10. Yaw Rate Gyro • Analog Devices AD22304 • It’s a +/-80 degree per second Gyro • It’s ‘0’ or no movement will give a reading of 2.5 volts. • 12.5 millivolts per degrees per second of rotation.

11. A/D port and Gyro • The A/D port is 10 bits. • We shift our reading over 6 bits to give us 16 bits. We do this for two reasons: • We have to do 16 bit math so lets use all 16 bits. • This allows us for “fractional” parts of the Gyro. The fractional parts are now the lower 6 bits of the number.

12. Our Gyro State Machine Start Take 512 readings. Sum up All the readings Get the average reading And make that the offset Take the reading, subtract The offset and add this rate To the running total Divide the running total by The scale and return that value

13. Our Gyro Setup • #define BITS 6 • #define GYROFRAC (1<<BITS) • #define FUDGE(A) (360.0/(float)A) // Put in actual reading to correct for 360 turn. • #define FULLSCALEINPUT (150.0 * 2.0 * FUDGE(280)) • #define FULLSCALEOUTPUT (1023.0 * (float)GYROFRAC) • #define GYRODELAY 512 // 5.12 second calibration period makes for simple math • // @100hz this is about 5-6000. Well within an int. • #define GYROSCALE ((FULLSCALEOUTPUT * GYROCLOCK)/FULLSCALEINPUT) • FULLSCALEINPUT = 385.714 • FULLSCALEOUTPUT = 65472 • GYROSCALE = 10863.51

14. The Code for State 3 • GyroVal = ((unsigned int)GetADC(Gyro)) * GYROFRAC; • #if GYRO_INVERTED==FALSE • rate = GyroVal - GyroOffset; • #else • rate = GyroOffset - GyroVal; • #endif • lRawTheta += rate; • iGyroTheta = lRawTheta / GYROSCALE;

15. Whoa our math is off • If the robot makes a 10 degree turn we wanted to see 10 degree from iGyroTheta • 10 degree * .0125V per D per S = .125V • Add that to the 2.5V for the middle = 2.625 • The A/D would be 537 • gyroVal = 537 * 64 = 34368 • Offset = 32768 so raw = 34368-32768 = 1600 • iGyroTheta = 1600/10863.51=.1472 • This should have given us 10.

16. Match the Observation • Adjust FULLSCALEINPUT & OUTPUT to match the observed valuesso that when the robot is turned 360 deg the reported angle is360. Double check by returning robot to starting position andverifying angle is 0. • This is where the wheels meet the road. • Here is also where Jim’s RC stopped working

17. Map • Primitive Encoder/Gyro • Dark Age Encoder (Quadrature) • Dark Age Gyro (Compass(ish)) • Renaissance Encoder • Renaissances Gyro • Enlightenment

18. Converting ticks to feet • // Wheels • #define WheelDiameter 6.0 • #define WheelWidth 26.0 • #define EncoderTicks (100.0*4.0) • #define GearRatio (32.0/48.0) • #define PI 3.14159 • #define TICKSPERINCH ((EncoderTicks * GearRatio) / (WheelDiameter * PI)) • #define DIST(A) ((int)(A)*TICKSPERINCH) • #define FEET(A) ((int)(A) * 12)

19. Map • Primitive Encoder/Gyro • Dark Age Encoder (Quadrature) • Dark Age Gyro (Compass(ish)) • Renaissance Encoder • Renaissances Gyro • Enlightenment

20. At this point we start using PIDish • We have a heading. • We have a gain, or how close should we be to the heading. • Lower number in the gain the less we pay attention to it. • Lower the gain for turning. • Raise the gain and the robot ‘snaps’ back into place

21. Quart’s Turn 2 Step 1

22. Quart’s Turn 2 Step 2

23. Quart’s Turn 2 Step 3

24. Quart’s Turn 2 Step 4

25. Setting the values • void SetTrackingAngle(int Angle) • { • TrackingAngle = Angle; • printf("Tracking Angle = %d\r\n", Angle); • } • void SetTrackingGain(int gain) • { • AngleKp = gain; • AngleKd = gain/2; • printf("TrackingGain = %d, %d\r\n", AngleKp, AngleKd); • }

26. Usin’ them values • // Track Gyro is a simple servo. It makes no adjustment for saturating the • // controller (i.e. setting a velocity > maximum) so keep the requested speed • // well under the maximum particuary if you want accurate tracking. • int PrevErr; • void TrackGyro(void) • { • if (AngleKp) • { • int err = TrackingAngle - Theta; • int Adjustment = (err * (AngleKp + AngleKd) - PrevErr * AngleKd)/ 16; • PrevErr = err; • printf("Trk %d, Th %d, AG %d Err %d\r\n", TrackingAngle, Theta, AngleKp, Adjustment); • Left.VelSp = RequestedVelocity + Adjustment; • Right.VelSp = RequestedVelocity - Adjustment; • } • }

27. Map • Primitive Encoder/Gyro • Dark Age Encoder (Quadrature) • Dark Age Gyro (Compass(ish)) • Renaissance Encoder • Renaissances Gyro • Enlightenment

28. First time in autonomous • if (FirstTime.Autonomous) • { • printf("Autonomous mode %x\r\n", AutoMode()); • FirstTime.Autonomous = FALSE; • FirstTime.Normal = TRUE; • // Reset servos. In autonomous mode, use Velocity • // control with integral error term. Effectively get • // position control. • SetMode(Velocity); • Left.IntError = Right.IntError = 0; • SetVelocity(0); • Left.Ki = Right.Ki = 0; // BUGBUG: What value? • // Grab starting position information • StartTimer(); • State = 1; • StartDistance = GetDistance(); • StartTheta = iGyroTheta; • }

29. Outside the normal state machine • // Each loop, calculate relative distance & angle from start. • Distance = GetDistance() - StartDistance; • Theta = iGyroTheta - StartTheta; • …….. • TrackGyro();

30. State 1, start ‘em up • case 1: • RequestedVelocity = 40; • SetTrackingAngle(0); • SetTrackingGain(10); • State = 2; • printf("State 1 finished\r\n"); • break;

31. State 2 First turn • case 2: • if (Distance > FEET(38)) • { • printf("State 2 satisfied @ %d\r\n", Distance); • SetTrackingAngle(-180); • SetTrackingGain(2); • StartDistance = GetDistance(); • State = 4; • } • break;

32. State 4 back stretch • case 4: • if (Distance > FEET(20) || Theta < -170) • { • StartDistance = GetDistance(); • SetTrackingGain(10); • State = 5; • } • break;

33. State 5 turn 2 • case 5: • if (Distance > FEET(18)) • { • SetTrackingGain(2); • StartDistance = GetDistance(); • SetTrackingAngle(-360); • State = 6; • } • break;

34. State 6 final stretch • case 6: • if (Distance > FEET(20) || Theta < -350) • { • StartDistance = GetDistance(); • SetTrackingGain(10); • State = 7; • } • break;

35. State 7 wait for the end • case 7: • if (Distance > FEET(10)) • { • State = 10; • } • break; • default: • SetVelocity(0); • SetTrackingGain(0); • break;

36. Um… Jim… the PID? • We have a servo struct for every motor. • Everything changes only the requested vel’. • Several different types, we use Velocity which falls into Torque • Everything is converted into a signed number: 0 to 255 becomes: -128 to 127

37. struct • typedef struct Servo • { • int Kp; // Position Error Gain (error->vel) • int Kv; // Velocity Error Gain (error->Torq) • int Ki; // Integral Error Gain • int Kemf; // Back-EMF gain Forward • int Kemb; // Back-EMF gain Reverse • char MinFwd; // De-deadband IFI controllers • char MinRev; • t_ServoMode Mode; • long PosSp; // Position Setpoint • long Position; // Current position • int Velocity; // Current Velocity • int VelSp; // Velocity Setpoint • int VelLim; // Velocity Limit • int VelReq; // Current Requested Velocity • int Accel; // Acceleration limit • int TorqSp; // Torque Setpoint • int TorqLim; // Torque Limit • int IntError; // Velocity Integral Error • int Drive; // Current drive value • char dP; • char dPabs; // Instability check • t_ServoMode PrevMode; • } • t_Servo, *pt_Servo;

38. Top of the PID loop • // Update structure with last measured Velocity • LowIntEnable(FALSE); • tVelocity = *Encoder; • *Encoder = 0; • LowIntEnable(TRUE); • p->Velocity = tVelocity; • p->Position += tVelocity;

39. Velosity State • if (p->VelSp > tVelocity) // Perform acceleration limits • { // VelReq is internal velocity. • p->VelReq = tVelocity + p->Accel; • if (p->VelReq > p->VelSp) • p->VelReq = p->VelSp; • } • else // if (p->VelSp < 0) • { • p->VelReq = tVelocity - p->Accel; • if (p->VelReq < p->VelSp) • p->VelReq = p->VelSp; • } • // Generate error term • VelocityError = p->VelReq - tVelocity; • NewTorque = ((VelocityError * KvSchedule(p)) + (((p->IntError * p->Ki)/16)))/16; • p->TorqSp = LimitStuff(NewTorque, p->TorqLim); • if (NewTorque == p->TorqSp) • { • p->IntError += VelocityError; // Should we limit integral to 255? • }

40. Fall into Torque • case Torque: • if (tVelocity > 0) • tKem = p->Kemf; • else • tKem = p->Kemb; • Drive = p->TorqSp + (tVelocity * tKem)/64; • break;

41. Handle the Drive Stuff • // De-deadband IFI speed controller output (account for slip/stick friction too) • if (Drive > 0) • { • Drive += p->MinFwd; • } • else if (Drive < 0) • { • Drive -= p->MinRev; • } • // Finally, convert to 0-254 output for IFI controllers. • return (p->Drive = 127 + LimitStuff(Drive, 127));

42. Calling this function • These calls take place after the Auto state machine • LeftCIM1 = LeftCIM2 = DoPid(&Left, &LeftEncoder); • RightCIM1 = RightCIM2 = DoPid(&Right, &RightEncoder);