A robot rover with distance sensing and audiovisual effects
Dependencies: 4DGL-uLCD-SE Motordriver PID mbed
Fork of PIDRover by
main.cpp
- Committer:
- Szilard
- Date:
- 2016-03-16
- Revision:
- 6:9dc165a89453
- Parent:
- 5:a8f6ac485b5d
File content as of revision 6:9dc165a89453:
/** * R2D2 is a mbed robot with the Shadowrobot chassis, two DC motors with feedback control, * IR distance sensor, a speaker and a uLCD */ #include "mbed.h" #include "motordriver.h" #include "PID.h" #include "uLCD_4DGL.h" #include "SongPlayer.h" //one full on this wheel is ~193 counts class Counter { //interrupt driven rotation counter to be used with the feedback control public: Counter(PinName pin) : _interrupt(pin) { // create the InterruptIn on the pin specified to Counter _interrupt.rise(this, &Counter::increment); // attach increment function of this counter instance } void increment() { _count++; } int read() { return _count; } private: InterruptIn _interrupt; volatile int _count; }; int distTransform(float input) { //stepwise transform the IR output voltage to distance if (input>3) return 6; //IR sensor datasheet: www.sharp-world.com/products/device/lineup/data/pdf/datasheet/gp2y0a21yk_e.pdf else if (input>2.5) return 8; else if (input>2) return 10; else if (input>1.5) return 14; else if (input>1.1) return 22; else if (input>0.9) return 27; else if (input>0.75) return 35; else if (input>0.6) return 45; else if (input>0.5) return 60; else if (input>0.4) return 75; else return 99; } Motor leftMotor(p22, p6, p5, 1); // pwm, fwd, rev, can brake Motor rightMotor(p21, p7, p8, 1); // pwm, fwd, rev, can brake Counter leftPulses(p9), rightPulses (p10); //Tuning parameters calculated from step tests; //see http://mbed.org/cookbook/PID for examples. PID leftPid(0.4620, 0.1, 0.0, 0.01); //Kc, Ti, Td PID rightPid(0.4620, 0.1, 0.0, 0.01); //Kc, Ti, Td DigitalOut led(LED1), led2(LED2); //LED feedback AnalogIn ain(p15); //A/D converter for the IR sensor uLCD_4DGL uLCD(p28,p27,p29); // serial tx, serial rx, reset pin for the uLCD float note[18]= {3520, 3135.96, 2637.02, 2093, 2349.32, 3951.07, 2793.83, 4186.01, 3520, 3135.96, 2637.02, 2093, 2349.32, 3951.07, 2793.83, 4186.01}; //R2D2 sound effect float duration[18]= {0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1}; //for a bit of variety, multiple sound samples could be chosen at random int main() { uLCD.printf("\n I am on an\n important\n mission!"); //Initialization //Tune PID controllers, based on mbed Rover: https://developer.mbed.org/cookbook/mbed-Rover leftPid.setInputLimits(0, 3000); leftPid.setOutputLimits(0.0, 0.8); leftPid.setMode(AUTO_MODE); rightPid.setInputLimits(0, 3000); rightPid.setOutputLimits(0.0, 0.8); rightPid.setMode(AUTO_MODE); Serial pc(USBTX, USBRX); SongPlayer mySpeaker(p23); int leftPrevPulses = 0, leftActPulses=0; //pulses from left motor float leftVelocity = 0.0; //The velocity of the left wheel in pulses per second int rightPrevPulses = 0, rightActPulses=0; //pulses from right motor float rightVelocity = 0.0; //The velocity of the right wheel in pulses per second int distance = 0; //Number of pulses to travel. led=0; led2=0; uLCD.baudrate(3000000); //uLCD baud rate for fast display wait(1); //Wait one second before we start moving. uLCD.cls(); uLCD.locate(1,2); uLCD.printf("I must find\n Ben Kenobi!"); //optional motor soft starting to reduce high inrush current /*leftMotor.speed(0.1); rightMotor.speed(0.1); wait(0.1);*/ leftPid.setSetPoint(1000); //set velocity goals for PID rightPid.setSetPoint(1000); while (1) { //start of big while loop if (distTransform(ain)>50) { //if no barrier within 50 cm go in a straight line! leftActPulses=leftPulses.read(); leftVelocity = (leftActPulses - leftPrevPulses) / 0.01; leftPrevPulses = leftActPulses; rightActPulses=rightPulses.read(); rightVelocity = (rightActPulses - rightPrevPulses) / 0.01; rightPrevPulses = rightActPulses; leftPid.setProcessValue(fabs(leftVelocity)); leftMotor.speed(leftPid.compute()); rightPid.setProcessValue(fabs(rightVelocity)); rightMotor.speed(rightPid.compute()); } else { //if there is a barrier within 50 cm, don't go straight, turn! leftMotor.stop(0.1); rightMotor.stop(0.1); led2=1; //turn on LED2 when it is turning uLCD.cls(); mySpeaker.PlaySong(note,duration); //play R2D2 sound effects uLCD.filled_circle(64, 64, 63, RED); //display R2D2 visual effects wait(0.2); uLCD.filled_circle(64, 64, 63, 0x0000FF); //light blue color wait(0.5); uLCD.filled_circle(64, 64, 63, RED); wait(0.3); //wait(0.5); leftActPulses=leftPulses.read(); rightActPulses=rightPulses.read(); distance=leftActPulses+100; while (leftActPulses<distance) { //turn approximately half a revolution leftMotor.speed(-0.5); //rotate to the right rightMotor.speed(0.5); //open loop, because the PID class can't handle negative values leftActPulses=leftPulses.read(); rightActPulses=rightPulses.read(); wait(0.005); }//Turning while end leftMotor.stop(0.1); rightMotor.stop(0.1); wait(0.1); led2=0; uLCD.cls(); uLCD.locate(1,2); uLCD.printf("I must find\n Ben Kenobi!"); leftPid.setSetPoint(1000); //go straight rightPid.setSetPoint(1000); } //Going straight/turning if end //pc.printf("\n%i", distTransform(ain)); //for debugging purposes you can read the distance reading //uLCD.locate(1,1); //uLCD.printf("Distance: %i cm", distTransform(ain)); wait(0.01); led=!led; //blink led1 to follow changes } //end of big while loop }