Committed for sharing.
Dependencies: mbed SelfBalancingRobot_BerkYasarYavuz_AdilBerkayTemiz Motor ledControl2 HC_SR04_Ultrasonic_Library
Revision 11:0a2944e7be23, committed 2020-01-01
- Comitter:
- berkyavuz1997
- Date:
- Wed Jan 01 20:31:22 2020 +0000
- Parent:
- 10:8050817ae610
- Commit message:
- Committed for sharing.
Changed in this revision
--- a/BalancingRobot.cpp Thu Aug 25 22:55:36 2016 +0000 +++ b/BalancingRobot.cpp Wed Jan 01 20:31:22 2020 +0000 @@ -8,48 +8,73 @@ #include "BalancingRobot.h" #include "MPU6050.h" #include "ledControl.h" -#include "HALLFX_ENCODER.h" +#include "ultrasonic.h" + +//#include "HALLFX_ENCODER.h" /* Setup serial communication */ -Serial blue(p28,p27); // For remote control ps3 +Serial blue(PTE22, PTE23); // For remote control ps3 Serial pc(USBTX, USBRX); // USB /* IMU */ -MPU6050 mpu6050; +MPU6050 mpu6050; + + -/* Encoders*/ -HALLFX_ENCODER leftEncoder(p13); -HALLFX_ENCODER rightEncoder(p14); +/* HC-SR04 */ +void dist(int distance) +{ + //put code here to happen when the distance is changed + +} + +ultrasonic frontUltra(PTA13, PTD5, .1, 1, &dist); +ultrasonic backUltra(PTD0, PTD2, .1, 1, &dist); /* Setup timer instance */ Timer t; /* Ticker for led toggling */ -Ticker toggler1; +Ticker toggler1; + + int main() { + + + + /* Set baudrate */ - blue.baud(9600); - pc.baud(9600); + blue.baud(115200); + pc.baud(115200); + /* Set PWM frequency */ leftPWM.period(0.00005); // The motor driver can handle a pwm frequency up to 20kHz (1/20000=0.00005) rightPWM.period(0.00005); + + /* Calibrate the gyro and accelerometer relative to ground */ 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"); + //pc.printf("Calibration is completed. \r\n"); mpu6050.init(); // Initialize the sensor - pc.printf("MPU6050 is initialized for operation.. \r\n\r\n"); + //pc.printf("MPU6050 is initialized for operation.. \r\n\r\n"); + + /* Setup timing */ t.start(); + loopStartTime = t.read_us(); timer = loopStartTime; - + DesiredAngle = -2.5; + ConstantAngle = DesiredAngle; + while (1) { + /* Calculate pitch angle using a Complementary filter */ mpu6050.complementaryFilter(&pitchAngle, &rollAngle); wait_us(5); @@ -59,9 +84,9 @@ if (abs(pitchAngle)> 20) // Stop if falling or laying down stopAndReset(); else - PID(targetAngle,targetOffset); - - /* Update wheel velocity every 100ms */ + PID(DesiredAngle, targetOffset); + + /* Update wheel velocity every 100ms loopCounter++; if (loopCounter%10 == 0) { // If remainder is equal 0, it must be 10,20,30 etc. @@ -77,20 +102,37 @@ stopped = true; } } + */ - pc.printf("Pitch: %f, error: %f, PIDValue: %f \n",pitchAngle,error,PIDValue); - pc.printf("Left Wheel Encoder: %f, Right Wheel Encoder: %f \n", LeftWheelPosition, RightWheelPosition); - pc.printf("Wheel Position: %f, Last wheel position:%f, Velocity: %f \n\n", wheelPosition, lastWheelPosition, wheelVelocity); + //pc.printf("Pitch: %f, error1: %f, PIDValue: %f \n\r",pitchAngle,error1,PIDValue); - /* Check for incoming serial data */ + //Check for incoming serial data if (blue.readable()) // Check if there's any incoming data receiveBluetooth(); - + + + + frontUltra.startUpdates(); + + distF = frontUltra.getCurrentDistance(); + + backUltra.startUpdates(); + + distB = backUltra.getCurrentDistance(); + + + pc.printf("Front distance: %d cm \t Back Distance %d cm \r\n", distF, distB); + /* Use a time fixed loop */ lastLoopUsefulTime = t.read_us() - loopStartTime; if (lastLoopUsefulTime < STD_LOOP_TIME) wait_us(STD_LOOP_TIME - lastLoopUsefulTime); + lastLoopTime = t.read_us() - loopStartTime; // only used for debugging + //pc.printf("STD_LOOP_TIME: %d\n\r",STD_LOOP_TIME); + //pc.printf("anlik: %d\n\r",t.read_us()); + //pc.printf("lastLoopTime: %d\n\r",lastLoopTime); + //pc.printf("lastLoopUsefulTime: %d\n\r",lastLoopTime); loopStartTime = t.read_us(); } } @@ -102,19 +144,26 @@ void PID(double restAngle, double offset) { + /* Steer robot */ - if (steerForward) + /*if (steerForward) { - offset += (double)wheelVelocity/velocityScaleMove; // Scale down offset at high speed and scale up when reversing - restAngle -= offset; - } + /* offset += (double)wheelVelocity/velocityScaleMove; // Scale down offset at high speed and scale up when reversing + restAngle -= offset; + restAngle -= -1.0; + + + } else if (steerBackward) { - offset -= (double)wheelVelocity/velocityScaleMove; // Scale down offset at high speed and scale up when reversing - restAngle += offset; - } + //offset -= (double)wheelVelocity/velocityScaleMove; // Scale down offset at high speed and scale up when reversing + //restAngle += offset; + restAngle += 1.0; + }*/ /* Brake */ - else if (steerStop) + /*else if (steerStop) + + restAngle = 0.0; { //long positionError = wheelPosition - targetPosition; @@ -125,28 +174,29 @@ else // Inside zone C restAngle -= (double)positionError/positionScaleC; */ - restAngle -= (double)wheelVelocity/velocityScaleStop; + //restAngle -= (double)wheelVelocity/velocityScaleStop; - if (restAngle < -5.00) // Limit rest Angle + /*if (restAngle < -5.00) // Limit rest Angle restAngle = -5.00; else if (restAngle > 5.00) restAngle = 5.00; } /* Update PID values */ - error = restAngle - pitchAngle; - double pTerm = Kp * error; - iTerm += Ki * error; - double dTerm = Kd * (error - lastError); - lastError = error; + error1 = restAngle - pitchAngle; + double pTerm = Kp * error1; + iTerm += Ki * error1; + double dTerm = Kd * (error1 - lastError); + lastError = error1; PIDValue = pTerm + iTerm + dTerm; - + //pc.printf("DesiredAngle = %lf", DesiredAngle); //pc.printf("Pitch: %5.2f\tPIDValue: %5.2f\tKp: %5.2f\tKi: %5.2f\tKd: %5.2f\n",pitch,PIDValue,Kp,Ki,Kd); /* Steer robot sideways */ double PIDLeft; - double PIDRight; + double PIDRight; + if (steerLeft) { PIDLeft = PIDValue-turnSpeed; PIDRight = PIDValue+turnSpeed; @@ -160,21 +210,39 @@ PIDLeft = PIDValue+rotateSpeed; PIDRight = PIDValue-rotateSpeed; } else { + + if (steerForward && (DesiredAngle > ( ConstantAngle - 1.8 ) )) + DesiredAngle -= rotateSpeed; + + else if (steerBackward && (DesiredAngle < (ConstantAngle + 1.8) )) + DesiredAngle += rotateSpeed; + + else { + if(!steerForward && DesiredAngle < ConstantAngle) + DesiredAngle += rotateSpeed; + + else if(!steerBackward && DesiredAngle > ConstantAngle) + DesiredAngle -= rotateSpeed; + } + PIDLeft = PIDValue; PIDRight = PIDValue; + } //PIDLeft *= 0.95; // compensate for difference in the motors /* Set PWM Values */ - if (PIDLeft >= 0) - move(left, forward, PIDLeft); + if (PIDLeft >= 0.0) + move(left, backward, PIDLeft + 0.55); else - move(left, backward, PIDLeft * -1); - if (PIDRight >= 0) - move(right, forward, PIDRight); + move(left, forward, (PIDLeft - 0.55) * -1); + if (PIDRight >= 0.0) + move(right, backward, PIDRight + 0.55); else - move(right, backward, PIDRight * -1); + move(right, forward, (PIDRight - 0.55) * -1); +//pc.printf("RestAngle3 %lf" ,restAngle); } + void receiveBluetooth() { //char input[16]; // The serial buffer is only 16 characters char phone_char; @@ -188,8 +256,24 @@ //pc.printf("Im here.."); //} phone_char = blue.getc(); + + /* if(distB < 50 && distB > 4) { + phone_char = 'S'; + if (distB < 20) + phone_char = 'F'; + } */ + + + if(distB < 20 && distB > 4) + phone_char = 'F'; + else if ( distB < 30) + phone_char = 'S'; + else if (distB < 50) + phone_char = 'B'; + + pc.putc(phone_char); - //pc.printf("Input: %c\n",i); + pc.printf("Input: %c\n",phone_char); // Set all false steerForward = false; @@ -205,11 +289,15 @@ //strtok(/*input*/phone_char, ","); // Ignore 'F' //targetOffset = atof((strtok(NULL, ";")); // read until the end and then convert from string to double //pc.printf("%f\n",targetOffset); // Print targetOffset for debugging + //DesiredAnglee = 0.8; + pc.printf("F"); steerForward = true; } else if (/*input[0]*/phone_char == 'B') { // Backward //strtok(/*input*/phone_char, ","); // Ignore 'B' - targetOffset = atof(strtok(NULL, ";")); // read until the end and then convert from string to double + //targetOffset = atof(strtok(NULL, ";")); // read until the end and then convert from string to double //pc.printf("%f\n",targetOffset); // Print targetOffset for debugging + //DesiredAnglee = -5.2; + pc.printf("B"); steerBackward = true; } else if (/*input[0]*/phone_char == 'L') { // Left if (/*input[0]*/phone_char == 'L') // Left Rotate @@ -224,10 +312,11 @@ } else if (/*input[0]*/phone_char == 'S') { // Stop steerStop = true; stopped = false; - targetPosition = wheelPosition; + //targetPosition = wheelPosition; } else if (/*input[0]*/phone_char == 'A') { // Abort stopAndReset(); while (blue.getc() != 'C'); // Wait until continue is send + } }
--- a/BalancingRobot.h Thu Aug 25 22:55:36 2016 +0000 +++ b/BalancingRobot.h Wed Jan 01 20:31:22 2020 +0000 @@ -6,18 +6,14 @@ BusOut LEDs(LED1, LED2, LED3, LED4); /* Left motor */ -DigitalOut leftA(p5); -DigitalOut leftB(p6); -PwmOut leftPWM(p23); +DigitalOut leftA(D6); +DigitalOut leftB(D5); +PwmOut leftPWM(D7); /* Right motor */ -DigitalOut rightA(p7); -DigitalOut rightB(p8); -PwmOut rightPWM(p24); - -/* Encoder variables */ -float LeftWheelPosition = 0.0; -float RightWheelPosition = 0.0; +DigitalOut rightA(D4); +DigitalOut rightB(D3); +PwmOut rightPWM(D2); // Results double accYangle; @@ -26,15 +22,24 @@ int16_t raw_gyroYrate; /* PID variables */ -double Kp = 0.5; //11 - 7 -double Ki = 0.00001; //1 -double Kd = 0.01; //12 + +double Kp = 0.085; //0.100; //0.0400; // 11 - 7 +double Ki = 0.00900; //0.005;//0.00320; //1 0.001 sanki gideri var mı ne +double Kd = 0.050;//0.0125 baya iyi //0.0333; //12 + +//double Kp = 0.185; //11 - 7 almost well //0.1 +//double Ki = 0.0000; //1 //0.005 integralli ilk iyi gibi +//double Kd = 0.1; //12 //0.01 + double PIDValue = 0.0; -double targetAngle = 1.05; +double DesiredAngle; +double ConstantAngle; +unsigned int distF; +unsigned int distB; double targetOffset = 0; float pitchAngle = 0; float rollAngle = 0; -double error = 0.0; +double error1 = 0.0; double lastError = 0.0; double iTerm;
--- a/HALLFX_ENCODER.cpp Thu Aug 25 22:55:36 2016 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,20 +0,0 @@ -#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
--- a/HALLFX_ENCODER.h Thu Aug 25 22:55:36 2016 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,38 +0,0 @@ -#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/HC_SR04_Ultrasonic_Library.lib Wed Jan 01 20:31:22 2020 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/ejteb/code/HC_SR04_Ultrasonic_Library/#e0f9c9fb4cf3
--- a/MPU6050.lib Thu Aug 25 22:55:36 2016 +0000 +++ b/MPU6050.lib Wed Jan 01 20:31:22 2020 +0000 @@ -1,1 +1,1 @@ -https://developer.mbed.org/users/lakshmananag/code/MPU6050/#c3d0207e85ca +https://os.mbed.com/teams/Berk-Ay/code/SelfBalancingRobot_BerkYasarYavuz_AdilBe/#cccf074499d0
--- a/mbed.bld Thu Aug 25 22:55:36 2016 +0000 +++ b/mbed.bld Wed Jan 01 20:31:22 2020 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/users/mbed_official/code/mbed/builds/14f4805c468c +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file