Lakshmanan Gopalakrishnan
Prototype program for balancing robot based on various examples from other users.
Dependencies: HCSR04 MPU6050_1 Motor Servo ledControl2 mbed
Revision 0:cfae0986065f, committed 2016-08-26
- Comitter:
- lakshmananag
- Date:
- Fri Aug 26 07:11:47 2016 +0000
- Commit message:
- Prototype program for balancing robot.
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/HALLFX_ENCODER.cpp Fri Aug 26 07:11:47 2016 +0000
@@ -0,0 +1,20 @@
+#include "HALLFX_ENCODER.h"
+
+HALLFX_ENCODER::HALLFX_ENCODER(PinName enc_in): _enc_in(enc_in){
+ _enc_in.mode(PullUp);
+ // Invoke interrupt on both falling and rising edges
+ _enc_in.fall(this, &HALLFX_ENCODER::callback);
+ _enc_in.rise(this, &HALLFX_ENCODER::callback);
+}
+
+long HALLFX_ENCODER::read(){
+ return count;
+}
+
+void HALLFX_ENCODER::reset(){
+ count = 0;
+}
+
+void HALLFX_ENCODER::callback(){
+ count++;
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/HALLFX_ENCODER.h Fri Aug 26 07:11:47 2016 +0000
@@ -0,0 +1,38 @@
+#ifndef HALLFX_ENCODER_H
+#define HALLFX_ENCODER_H
+
+/*
+ Basic Encoder Library for Sparkfun's Wheel Encoder Kit
+ Part# ROB-12629.
+*/
+
+#include "mbed.h"
+
+class HALLFX_ENCODER{
+ public:
+ /*
+ Constructor for Encoder objects
+ @param enc_in The mBed pin connected to encoder output
+ */
+ HALLFX_ENCODER(PinName enc_in);
+ /*
+ read() returns total number of counts of the encoder.
+ Count can be +/- and indicates the overall direction,
+ (+): CW (-): CCW
+ @return The toltal number of counts of the encoder.
+ */
+ long read();
+ /*
+ reset() clears the counter to 0.
+ */
+ void reset();
+ private:
+ long count; // Total number of counts since start.
+ InterruptIn _enc_in;// Encoder Input/Interrupt Pin
+ /*
+ Increments/Decrements count on interrrupt.
+ */
+ void callback(); // Interrupt callback function
+};
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/HCSR04.lib Fri Aug 26 07:11:47 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/prabhuvd/code/HCSR04/#71da0dbf4400
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MPU6050.lib Fri Aug 26 07:11:47 2016 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/users/lakshmananag/code/MPU6050_1/#f434122e7695
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Motor.lib Fri Aug 26 07:11:47 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/simon/code/Motor/#f265e441bcd9
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Servo.lib Fri Aug 26 07:11:47 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/simon/code/Servo/#36b69a7ced07
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ledControl.lib Fri Aug 26 07:11:47 2016 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/users/BaserK/code/ledControl2/#1655ee0ec2ca
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Aug 26 07:11:47 2016 +0000
@@ -0,0 +1,210 @@
+#include "mbed.h"
+#include "hcsr04.h"
+#include "MPU6050.h"
+#include "ledControl.h"
+#include "Motor.h"
+#include "HALLFX_ENCODER.h"
+
+Serial pc(USBTX, USBRX); // Create terminal link
+Serial blue(p28, p27); // Bluetooth TX, RX
+
+MPU6050 mpu6050; // mpu6050 object from MPU6050 classs
+
+Ticker toggler1; // Ticker for led toggling
+Ticker filter; // Ticker for periodic call to compFilter funcçs
+Ticker balance; // Periodic routine for PID control of balance system
+Ticker speed; // Periodic routine for speed control
+Ticker bluetooth; // Ticker for navigation
+
+Motor A(p23, p5, p6); // pwm, fwd, rev
+Motor B(p24, p7, p8); // pwm, fwd, rev
+
+/* Encoders*/
+HALLFX_ENCODER leftEncoder(p13);
+HALLFX_ENCODER rightEncoder(p14);
+
+HCSR04 usensor(p25,p26); //trig pin,echo pin
+
+/* Function prototypes */
+void toggle_led1();
+void toggle_led2();
+void compFilter();
+void balancePID();
+void balanceControl();
+void Forward(float);
+void Reverse(float);
+void Stop(void);
+void Navigate();
+void TurnRight(float);
+void TurnLeft(float);
+
+/* Variable declarations */
+float pitchAngle = 0;
+float rollAngle = 0;
+bool dir; // direction of movement
+float Kp = 0.5;
+float Ki = 0.00001;
+float Kd = 0.01;//0.01;//0.05;
+float set_point = 1.005; // Angle for staying upright
+float errorPID = 0; // Proportional term (P) in PID
+float last_errorPID =0; // Previous error value
+float integral = 0; // Integral (I)
+float derivative = 0; // Derivative (D)
+float outputPID = 0; // Output of PID calculations
+float position = 0.0;
+float dist=0.0;
+float range = 0.000;
+int phone_char;
+DigitalOut myled(LED4);
+
+int main()
+{
+ pc.baud(9600); // baud rate: 9600
+ blue.baud(9600);
+
+ mpu6050.whoAmI(); // Communication test: WHO_AM_I register reading
+ wait(0.5);
+ mpu6050.calibrate(accelBias,gyroBias); // Calibrate MPU6050 and load biases into bias registers
+ pc.printf("Calibration is completed. \r\n");
+ mpu6050.init(); // Initialize the sensor
+ pc.printf("MPU6050 is initialized for operation.. \r\n\r\n");
+
+ filter.attach(&compFilter, 0.005); // Call the complementaryFilter func. every 5 ms (200 Hz sampling period)
+ balance.attach(&balancePID, 0.010); // Same period with balancePID
+ speed.attach(&balanceControl, 0.01);
+ bluetooth.attach(&Navigate, 0.05);
+
+ while(1)
+ {
+
+ if(blue.readable())
+ {
+ phone_char = blue.getc();
+ pc.putc(phone_char);
+ pc.printf("Bluetooth Start\r\n");
+ myled = !myled;
+ }
+
+ pc.printf("Roll Angle: %.1f, Pitch Angle: %.1f\r\n",rollAngle,pitchAngle);
+ pc.printf("Error = %f\r\n",outputPID);
+ wait_ms(40);
+ }
+}
+
+void toggle_led1() {ledToggle(1);}
+void toggle_led2() {ledToggle(2);}
+
+/* This function is calls the complementary filter with pitch and roll angles */
+void compFilter() {mpu6050.complementaryFilter(&pitchAngle, &rollAngle);}
+
+void balancePID()
+{
+ errorPID = set_point - pitchAngle; //Proportional (P)
+ integral += errorPID; //Integral (I)
+ derivative = errorPID - last_errorPID; //Derivative (D)
+
+ last_errorPID = errorPID; //Save current value for next iteration
+
+ outputPID = (Kp * errorPID) + (Ki * integral) + (Kd * derivative); //PID calculation
+
+ /* errorPID is restricted between -1 and 1 */
+ if(outputPID > 0.8)
+ outputPID = 0.8;
+ else if(outputPID < -0.8)
+ outputPID = -0.8;
+}
+
+void balanceControl()
+{
+ int direction=0; // Variable to hold direction we want to drive
+ if (outputPID>0)direction=1; // Positive speed indicates forward
+ if (outputPID<0)direction=2; // Negative speed indicates backwards
+ if((abs(pitchAngle)>10.00))direction=0;
+
+ switch( direction) { // Depending on what direction was passed
+ case 0: // Stop case
+ Stop();
+ break;
+ case 1: // Forward case
+ Forward(abs(outputPID));
+ break;
+ case 2: // Backwards
+ Reverse(abs(outputPID));
+ break;
+ default: // Catch-all (Stop)
+ Stop();
+ break;
+ }
+}
+
+void Stop(void)
+{
+ A.speed(0.0);
+ B.speed(0.0);
+}
+
+void Forward(float fwd_dist)
+ {
+ A.speed(-fwd_dist);
+ B.speed(-fwd_dist);
+ }
+
+void TurnRight(float ryt_dist)
+ {
+ A.speed(ryt_dist);
+ B.speed(-ryt_dist);
+ }
+
+void TurnLeft(float lft_dist)
+ {
+ A.speed(-lft_dist);
+ B.speed(lft_dist);
+ }
+
+void Reverse(float rev_dist)
+ {
+ A.speed(rev_dist);
+ B.speed(rev_dist);
+ }
+
+ void Navigate()
+ {
+ switch (phone_char)
+ {
+ case 'f':
+ Forward(0.7);
+ break;
+
+ case 'F':
+ Forward(0.7);
+ break;
+
+ case 'b':
+ Reverse(0.7);
+ break;
+
+ case 'B':
+ Reverse(0.7);
+ break;
+
+ case 'r':
+ TurnRight(0.7);
+ break;
+
+ case 'R':
+ TurnRight(0.7);
+ break;
+
+ case 'l':
+ TurnLeft(0.7);
+ break;
+
+ case 'L':
+ TurnLeft(0.7);
+ break;
+
+ default:
+ Stop();
+ }
+ }
+
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri Aug 26 07:11:47 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/4f6c30876dfa \ No newline at end of file