Adrian Winata
Use the IMU to correct steering on the Sparkfun magician robot. Moreover, IR sensors will be used to avoid and navigate around obstacles.
Dependencies: LSM9DS0 Motordriver mbed
Revision 0:98baffd99476, committed 2015-03-11
- Comitter:
- awinata
- Date:
- Wed Mar 11 21:57:57 2015 +0000
- Commit message:
- Initial release.
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LSM9DS0.lib Wed Mar 11 21:57:57 2015 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/aswild/code/LSM9DS0/#5556e6fb99f5
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Motordriver.lib Wed Mar 11 21:57:57 2015 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/littlexc/code/Motordriver/#3110b9209d3c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Mar 11 21:57:57 2015 +0000
@@ -0,0 +1,211 @@
+#include "mbed.h"
+#include "motordriver.h"
+#include "LSM9DS0.h"
+
+/*/////// PID control gains
+ / These values need to be adjusted in accordance with the individual
+ / actuators (motors) either by trial and error or computation. The information
+ / here should help: http://developer.mbed.org/cookbook/PID
+*////////
+#define RATE 0.1
+#define Kp 0.0
+#define Ki 0.0
+#define Kd 0.0
+
+// SDO_XM and SDO_G are pulled up, so our addresses are:
+#define LSM9DS0_XM_ADDR 0x1D // Would be 0x1E if SDO_XM is LOW
+#define LSM9DS0_G_ADDR 0x6B // Would be 0x6A if SDO_G is LOW
+
+// peripheral objects
+DigitalOut f(LED1); // forward
+DigitalOut b(LED2); // backward
+DigitalOut l(LED3); // left
+DigitalOut r(LED4); // right
+//See http://mbed.org/cookbook/Motor
+//Connections to dual H-brdige driver for the two drive motors
+Motor left(p21, p22, p23, 1); // pwm, fwd, rev, has brake feature
+Motor right(p26, p25, p24, 1);
+AnalogIn ir_c(p20); // center IR sensor
+AnalogIn ir_l(p19); // left IR sensor
+AnalogIn ir_r(p18); // right IR sensor
+LSM9DS0 imu(p9, p10, LSM9DS0_G_ADDR, LSM9DS0_XM_ADDR); // imu sensor
+Serial pc(USBTX, USBRX); // usb serial for debugging
+
+// function prototypes
+void setup();
+void initIMU();
+float getHeading();
+float updatePID();
+
+// global variables
+int count; // counter for IR
+float initHeading; // initial heading
+float calcHeading; // calculated heading
+float heading[1]; // index 0 = goal, index 1 = current
+float headingAvg[100];
+float sum;
+float previousError; // previous error
+float pidError; // error
+float steadyError; // steady-state error
+float P; // proportional error
+float I; // integral error
+float D; // derivative error
+float dt; // update frequency
+float kp; // proportional constant
+float ki; // integral constant
+float kd; // derivative constant
+float output; // PID controller output
+float motorSpeed1; // motor speed for left motor
+float motorSpeed2; // motor speed for right motor
+
+// function implementations
+void setup()
+{
+ // Use the begin() function to initialize the LSM9DS0 library.
+ // You can either call it with no parameters (the easy way):
+ uint16_t status = imu.begin();
+
+ //Make sure communication is working
+ pc.printf("LSM9DS0 WHO_AM_I's returned: 0x%X\n", status);
+ pc.printf("Should be 0x49D4\n\n");
+}
+
+// obtains the initial heading upon bootup
+void initIMU()
+{
+ imu.readMag();
+ initHeading = imu.calcHeading();
+ heading[0] = initHeading;
+ heading[1] = initHeading;
+}
+
+// obtains an updated heading
+float getHeading()
+{
+ imu.readMag(); // reads current value
+ calcHeading = imu.calcHeading(); // sets current value
+
+ //pc.printf("Compass Heading (in degrees): ");
+ //pc.printf("%2f\r\n",calcHeading);
+ //pc.printf("%2f\r\n",heading[0]);
+
+ return calcHeading;
+}
+
+// pid controller
+float updatePID(float current, float target, float dt)
+{
+ // calculate difference between actual and goal values
+ pidError = target - current;
+ if (pidError < -270) pidError += 360;
+ if (pidError > 270) pidError -= 360;
+
+ // accumulates error over time
+ steadyError += pidError*dt;
+
+ // integrator windup compensation (saturates to actuator's output)
+ if (steadyError < -10.0) steadyError = -10.0;
+ if (steadyError > 10.0) steadyError = 10.0;
+
+ P = kp*pidError; // proportional error
+ I = ki*steadyError; // integral error
+ D = kd*(pidError - previousError)/dt; // derivative error
+
+ // save current error
+ previousError = pidError;
+
+ float pidOutput = P + I + D; // calculate the PID output
+ pidOutput /= 100.0; // scale it down to get it within the range of the actuator - probably needs tuning
+ if(pidOutput < -0.1) pidOutput = -0.1;
+ if(pidOutput > 0.1) pidOutput = 0.1;
+
+ wait(dt);
+
+ //pc.printf("P = %f | I = %f | D = %f | Output = %f\n",P,I,D,pidOutput);
+ return pidOutput;
+}
+
+int main()
+{
+ // initialization functions
+ setup();
+ initIMU();
+
+ // variables
+ dt = RATE;
+ kp = Kp;
+ ki = Ki;
+ kd = Kd;
+ motorSpeed1 = 0.4;
+ motorSpeed2 = 0.4;
+
+ while(1) {
+ if(ir_c > 0.25f) { // Turn around if about to hit an obstacle
+ f = 0;
+ b = 1;
+ l = 1;
+ r = 0;
+ while(count < 50) {
+ left.speed(0.5);
+ right.speed(-0.5);
+ wait(0.02);
+ count++;
+ }
+ count = 0;
+ heading[0] = getHeading(); // updates new goal heading
+ } else if(ir_l > 0.25f) { // Turn right if about to hit an obstacle
+ f = 0;
+ b = 0;
+ l = 1;
+ r = 0;
+ while(count < 20) {
+ left.speed(0.5);
+ right.speed(-0.5);
+ wait(0.02);
+ count++;
+ }
+ count = 0;
+ heading[0] = getHeading(); // updates new goal heading
+ } else if(ir_r > 0.25f) { // Turn left if about to hit an obstacle
+ f = 0;
+ b = 0;
+ l = 0;
+ r = 1;
+ while(count < 20) {
+ left.speed(-0.5);
+ right.speed(0.5);
+ wait(0.02);
+ count++;
+ }
+ count = 0;
+ heading[0] = getHeading(); // updates new goal heading
+ } else { // Moves forward indefinitely
+ f = 1;
+ b = 0;
+ l = 0;
+ r = 0;
+
+ // average the headings to elliminate noise (LPF)
+ // possibly needed to help smooth out sensor noise
+ /*
+ for(int x = 0; x < 100; x++) {
+ headingAvg[x] = getHeading();
+ sum += headingAvg[x];
+ }
+ heading[1] = sum/100;
+ */
+
+ // updates new pid values
+ heading[1] = getHeading();
+ output = updatePID(heading[1],heading[0],dt);
+
+ // updates new motor speeds
+ motorSpeed1 += output;
+ motorSpeed2 -= output;
+
+ // set motor speeds
+ left.speed(motorSpeed1);
+ right.speed(motorSpeed2);
+ }
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Wed Mar 11 21:57:57 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/7e07b6fb45cf \ No newline at end of file