Code for autonomous rover for Sparkfun AVC. DataBus won 3rd in 2012 and the same code was used on Troubled Child, a 1986 Jeep Grand Wagoneer to win 1st in 2014.
Dependencies: mbed Watchdog SDFileSystem DigoleSerialDisp
Actuators/Steering/Steering.h@15:01fb4916a5cd, 2018-11-29 (annotated)
- Committer:
- shimniok
- Date:
- Thu Nov 29 17:11:34 2018 +0000
- Revision:
- 15:01fb4916a5cd
- Parent:
- 2:fbc6e3cf3ed8
temp fix for setSteering() compile err in shell.cpp; updated logging class
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
shimniok | 2:fbc6e3cf3ed8 | 1 | #ifndef __STEERING_H |
shimniok | 2:fbc6e3cf3ed8 | 2 | #define __STEERING_H |
shimniok | 0:a6a169de725f | 3 | |
shimniok | 2:fbc6e3cf3ed8 | 4 | #include "globals.h" |
shimniok | 15:01fb4916a5cd | 5 | #include "devices.h" |
shimniok | 15:01fb4916a5cd | 6 | #include "Servo.h" |
shimniok | 15:01fb4916a5cd | 7 | #include "Config.h" |
shimniok | 0:a6a169de725f | 8 | |
shimniok | 0:a6a169de725f | 9 | /** A class for managing steering angle calculations based on current and desired heading |
shimniok | 0:a6a169de725f | 10 | * and specified intercept distance along the new path. |
shimniok | 0:a6a169de725f | 11 | * |
shimniok | 0:a6a169de725f | 12 | * See Notebook entry: http://mbed.org/users/shimniok/notebook/smooth-steering-for-rc-car/ |
shimniok | 0:a6a169de725f | 13 | */ |
shimniok | 0:a6a169de725f | 14 | class Steering |
shimniok | 0:a6a169de725f | 15 | { |
shimniok | 0:a6a169de725f | 16 | public: |
shimniok | 15:01fb4916a5cd | 17 | /** create a new steering calculator */ |
shimniok | 15:01fb4916a5cd | 18 | Steering(PinName pin); |
shimniok | 15:01fb4916a5cd | 19 | |
shimniok | 15:01fb4916a5cd | 20 | /** Initalize steering actuator (servo) |
shimniok | 15:01fb4916a5cd | 21 | * |
shimniok | 15:01fb4916a5cd | 22 | */ |
shimniok | 15:01fb4916a5cd | 23 | void initSteering(void); |
shimniok | 15:01fb4916a5cd | 24 | |
shimniok | 15:01fb4916a5cd | 25 | |
shimniok | 15:01fb4916a5cd | 26 | /** Set the track width (left to right contact patch distance) |
shimniok | 0:a6a169de725f | 27 | * |
shimniok | 15:01fb4916a5cd | 28 | * @param track is the vehicle track width |
shimniok | 0:a6a169de725f | 29 | */ |
shimniok | 15:01fb4916a5cd | 30 | void setTrack(float track); |
shimniok | 15:01fb4916a5cd | 31 | |
shimniok | 15:01fb4916a5cd | 32 | |
shimniok | 15:01fb4916a5cd | 33 | /** Set the wheelbase (front to rear axle distance) |
shimniok | 15:01fb4916a5cd | 34 | * |
shimniok | 15:01fb4916a5cd | 35 | * @param wheelbase is the vehicle wheelbase |
shimniok | 15:01fb4916a5cd | 36 | */ |
shimniok | 15:01fb4916a5cd | 37 | void setWheelbase(float wheelbase); |
shimniok | 15:01fb4916a5cd | 38 | |
shimniok | 0:a6a169de725f | 39 | |
shimniok | 0:a6a169de725f | 40 | /** set intercept distance |
shimniok | 0:a6a169de725f | 41 | * @param intercept distance along new course at which turn arc will intercept |
shimniok | 0:a6a169de725f | 42 | */ |
shimniok | 0:a6a169de725f | 43 | void setIntercept(float intercept); |
shimniok | 0:a6a169de725f | 44 | |
shimniok | 15:01fb4916a5cd | 45 | |
shimniok | 15:01fb4916a5cd | 46 | /** set steering scale factor to convert between steering angle and servo output |
shimniok | 15:01fb4916a5cd | 47 | * @param scale is the scale factor |
shimniok | 15:01fb4916a5cd | 48 | */ |
shimniok | 15:01fb4916a5cd | 49 | void setScale(float scale); |
shimniok | 15:01fb4916a5cd | 50 | |
shimniok | 15:01fb4916a5cd | 51 | |
shimniok | 15:01fb4916a5cd | 52 | /** Convert steerAngle to servo value |
shimniok | 15:01fb4916a5cd | 53 | * |
shimniok | 15:01fb4916a5cd | 54 | * Testing determined near linear conversion between servo ms setting and steering angle |
shimniok | 15:01fb4916a5cd | 55 | * up to ~20*. |
shimniok | 15:01fb4916a5cd | 56 | * |
shimniok | 15:01fb4916a5cd | 57 | * @param steerAngle is the steering angle, measured at front wheels, averaged |
shimniok | 15:01fb4916a5cd | 58 | */ |
shimniok | 15:01fb4916a5cd | 59 | void setSteering(float steerAngle); |
shimniok | 15:01fb4916a5cd | 60 | |
shimniok | 15:01fb4916a5cd | 61 | |
shimniok | 15:01fb4916a5cd | 62 | /** Shorthand for the write function */ |
shimniok | 15:01fb4916a5cd | 63 | float operator= (float steerAngle); |
shimniok | 15:01fb4916a5cd | 64 | |
shimniok | 15:01fb4916a5cd | 65 | |
shimniok | 0:a6a169de725f | 66 | /** convert course change to average steering angle |
shimniok | 0:a6a169de725f | 67 | * assumes Ackerman steering, with track and wheelbase |
shimniok | 0:a6a169de725f | 68 | * and course intercept distance specified. |
shimniok | 0:a6a169de725f | 69 | * |
shimniok | 0:a6a169de725f | 70 | * See notebook: http://mbed.org/users/shimniok/notebook/smooth-steering-for-rc-car/ |
shimniok | 0:a6a169de725f | 71 | * |
shimniok | 0:a6a169de725f | 72 | * @param theta relative bearing of the new course |
shimniok | 0:a6a169de725f | 73 | * @returns steering angle in degrees |
shimniok | 0:a6a169de725f | 74 | */ |
shimniok | 0:a6a169de725f | 75 | float calcSA(float theta); |
shimniok | 0:a6a169de725f | 76 | |
shimniok | 15:01fb4916a5cd | 77 | |
shimniok | 0:a6a169de725f | 78 | /** convert course change to average steering angle |
shimniok | 0:a6a169de725f | 79 | * assumes Ackerman steering, with track and wheelbase |
shimniok | 0:a6a169de725f | 80 | * and course intercept distance specified. Also, |radius| of turn is limited to limit |
shimniok | 0:a6a169de725f | 81 | * |
shimniok | 0:a6a169de725f | 82 | * See notebook: http://mbed.org/users/shimniok/notebook/smooth-steering-for-rc-car/ |
shimniok | 0:a6a169de725f | 83 | * |
shimniok | 0:a6a169de725f | 84 | * @param theta relative bearing of the new course |
shimniok | 0:a6a169de725f | 85 | * @param limit is the limit of the turn circle radius (absolute value) |
shimniok | 0:a6a169de725f | 86 | * @returns steering angle in degrees |
shimniok | 0:a6a169de725f | 87 | */ |
shimniok | 0:a6a169de725f | 88 | float calcSA(float theta, float limit); |
shimniok | 0:a6a169de725f | 89 | |
shimniok | 15:01fb4916a5cd | 90 | |
shimniok | 0:a6a169de725f | 91 | /** compute steering angle based on pure pursuit algorithm |
shimniok | 0:a6a169de725f | 92 | */ |
shimniok | 0:a6a169de725f | 93 | float purePursuitSA(float hdg, float Bx, float By, float Ax, float Ay, float Cx, float Cy); |
shimniok | 0:a6a169de725f | 94 | |
shimniok | 15:01fb4916a5cd | 95 | |
shimniok | 0:a6a169de725f | 96 | /** compute steering angle based on a simpler path pursuit variant of pure pursuit |
shimniok | 0:a6a169de725f | 97 | */ |
shimniok | 0:a6a169de725f | 98 | float pathPursuitSA(float hdg, float Bx, float By, float Ax, float Ay, float Cx, float Cy); |
shimniok | 0:a6a169de725f | 99 | |
shimniok | 15:01fb4916a5cd | 100 | |
shimniok | 0:a6a169de725f | 101 | /** Compute cross track error given last waypoint, next waypoint, and robot coordinates |
shimniok | 0:a6a169de725f | 102 | * @returns cross track error |
shimniok | 0:a6a169de725f | 103 | */ |
shimniok | 0:a6a169de725f | 104 | float crossTrack(float Bx, float By, float Ax, float Ay, float Cx, float Cy); |
shimniok | 15:01fb4916a5cd | 105 | |
shimniok | 0:a6a169de725f | 106 | |
shimniok | 15:01fb4916a5cd | 107 | /** Convert degrees to radians |
shimniok | 15:01fb4916a5cd | 108 | * @param deg degrees to convert |
shimniok | 15:01fb4916a5cd | 109 | * @return radians |
shimniok | 15:01fb4916a5cd | 110 | */ |
shimniok | 15:01fb4916a5cd | 111 | inline static float toRadians(float deg) {return (PI/180.0)*deg;} |
shimniok | 15:01fb4916a5cd | 112 | |
shimniok | 0:a6a169de725f | 113 | |
shimniok | 15:01fb4916a5cd | 114 | /** Convert radians to degrees |
shimniok | 15:01fb4916a5cd | 115 | * @param rad radians to convert |
shimniok | 15:01fb4916a5cd | 116 | * @return degrees |
shimniok | 15:01fb4916a5cd | 117 | */ |
shimniok | 15:01fb4916a5cd | 118 | inline static float toDegrees(float rad) {return (180/PI)*rad;} |
shimniok | 0:a6a169de725f | 119 | |
shimniok | 15:01fb4916a5cd | 120 | private: |
shimniok | 15:01fb4916a5cd | 121 | Servo _steering; // Steering Servo |
shimniok | 15:01fb4916a5cd | 122 | float _scale; // Steering scale factor |
shimniok | 15:01fb4916a5cd | 123 | float _track; // vehicle track used for steering intercept calc |
shimniok | 15:01fb4916a5cd | 124 | float _wheelbase; // vehicle wheelbase used for steering intercept calc |
shimniok | 15:01fb4916a5cd | 125 | float _intercept; // circle intercept distance |
shimniok | 2:fbc6e3cf3ed8 | 126 | }; |
shimniok | 2:fbc6e3cf3ed8 | 127 | |
shimniok | 15:01fb4916a5cd | 128 | #endif |