Mechatronics Robotics
/
BrobotV1
These are the core files for the Robot at Team conception.
Diff: Brobot.cpp
- Revision:
- 15:4efc66de795a
- Parent:
- 8:351b0b7b05b2
- Child:
- 17:ec52258b9472
diff -r 189b50e6b7c9 -r 4efc66de795a Brobot.cpp --- a/Brobot.cpp Mon May 29 13:10:00 2017 +0000 +++ b/Brobot.cpp Mon Jun 05 07:56:08 2017 +0000 @@ -5,69 +5,184 @@ #include <cmath> #include "Brobot.h" -#include "SpeedControl.h" + +Brobot::Brobot(SpeedControl& speedctrl_, AnalogIn& distance_, DigitalOut& enable_, DigitalOut& bit0_, DigitalOut& bit1_, DigitalOut& bit2_, DigitalOut* led_ptr, Pixy& pixy_, UniServ& servo_) : + speedctrl(speedctrl_), distance(distance_), enable(enable_), bit0(bit0_), bit1(bit1_), bit2(bit2_), leds(led_ptr), pixy(pixy_), servo(servo_) // assigning parameters to class variables +{ + //initialize distance sensors + for( int ii = 0; ii<6; ++ii) + sensors[ii].init(&distance, &bit0, &bit1, &bit2, ii); + + //defining the sensors for + sensor_front.init( &distance, &bit0, &bit1, &bit2, 0); // & give the address of the object + sensor_left.init( &distance, &bit0, &bit1, &bit2, 5); + sensor_right.init( &distance, &bit0, &bit1, &bit2, 1); + + // kp ki kd min max + //pid.setPIDValues( 0.025f, 0.00005f, 0.001f, 0.5f, -0.5f, 1000); + pid.setPIDValues( 0.4f, 0.001f, 0.001f, 0.5f, -0.5f, 1000); + pixypid.setPIDValues( 0.0002f, 0.000f, 0.000f, 0.3f, -0.3f, 1000); -SpeedControl speedctrl(PwmOut* pwmLeft, PwmOut* pwmRight, EncoderCounter* counterLeft, EncoderCounter* counterRight); +} + +void Brobot::avoidObstacleAndMove(int vtrans) +{ + float vrot = 0; // rpm + static float e = 0.0f ; + + if(sensor_left.read() <0.3f || sensor_right.read() <0.3f) { // to avoid obstacles + + e = sensor_left.read() - sensor_right.read(); + float diff = pid.calc( e, 0.05f ); // error and period are arguments + + vrot = diff*50; //turn + + if( vrot<= -20) + vrot=-20; + else if (vrot > 20) + vrot=20; + } -Brobot::Brobot(SpeedControl* speedctrl, int number) + speedctrl.setDesiredSpeed( vtrans - vrot, -vtrans - vrot ); + int delay = vtrans/100 ; + + if(sensor_front.read() <=0.25f) { // when approaching normal to wall + speedctrl.setDesiredSpeed( 0, 0 ); + + int direction=rand()%2 ; // so there is variablility in the robots path + if(direction==0) { + //stingray.rotate(10); + speedctrl.setDesiredSpeed(15, 15 ); + wait(delay); + } else if (direction==1) { + //stingray.rotate(-10); + speedctrl.setDesiredSpeed( -15, -15 ); + wait(delay); + } + } +} + +void Brobot::startLeds() { - this->number = number; - //this->speedctrl = speedctrl; + t1.attach( callback(this, &Brobot::ledShow), 0.05f ); +} + +void Brobot::ledDistance() +{ + for( int ii = 0; ii<6; ++ii) + sensors[ii]< 0.1f ? leds[ii].write(1) : leds[ii].write(0); // an if statement in one line +} + +void Brobot::ledShow() +{ + static int timer = 0; // static is only the first time/loop + // for( int ii = 0; ii<6; ++ii) + // leds[ii] = !leds[ii]; + + //quit ticker and start led distance show + if( ++timer > 10) { + t1.detach(); + t1.attach( callback(this, &Brobot::ledDistance), 0.01f ); + } } -void Brobot::rotate( float phi) +bool Brobot::foundGreenBrick() +{ + + if( pixy.objectDetected() ) { + if( pixy.getSignature() == 1) { // 1 is the green brick + return true; + } + } + return false; +} + +bool Brobot::foundHome() { - if(phi>0.5f || phi<-0.5f) { - phi=0; + if( pixy.objectDetected() ) { + if( pixy.getSignature() == 2) { // 2 is the home + return true; + } } + return false; +} - *pwmL = 0.5f + phi; - *pwmR = 0.5f + phi; +void Brobot::approachBrick() +{ + // Crystal's code + float e = 160-pixy.getX(); + float diff = pixypid.calc( e, 0.005f ); + speedctrl.setDesiredSpeed( -(diff*50.0f), -(diff*50.0f)) ; + // function assumes Brick is in open grips } -/* -void Brobot::setWheelSpeeds( float wheelL, float wheelR) +bool Brobot::approachHome() { - SpeedCtrl.setDesiredSpeed( wheelL, wheelR); + float x_e, diff ; + int triggered_sensors = 0, rotate_count = 0; + bool home_reached = false; + //the time to rotate after the brick is released and the distance away from + // the way that the robot should stop at + float sensor_trigger_dist=0.3; + + while(!home_reached) { + // find the error in the x alignment and calculate the restoring voltage + x_e = pixy.getX()-160; + diff = 50*pid.calc(x_e,0.05); + + // if home is detected drive the robot forward while continuously realigning the robot + if (pixy.objectDetected() && (pixy.getSignature() == 2)) { + speedctrl.setDesiredSpeed(15-diff,-15-diff); + rotate_count = 0; + } + // check the distance sensors. Home is reached when 2 sensors read < 0.3m + triggered_sensors = (sensor_left<sensor_trigger_dist)+(sensor_right<sensor_trigger_dist)+(sensor_front<sensor_trigger_dist); + if (triggered_sensors >= 2) + home_reached = true; + + // if home is not detected spin CW and check again + if (!pixy.objectDetected() || (pixy.objectDetected() && pixy.getSignature() == 1)) { + Brobot::rotate(5); + rotate_count++; + if (rotate_count > 10) + break; //break the loop if the robot rotates enough times in a row without detecting home + } + } + return home_reached; } -*/ + +void Brobot::closeGrip() +{ + int close=600; + servo.write_us(close); +} + +void Brobot::openGrip() +{ + int open=2100; + servo.write_us(open); +} + + +void Brobot::rotate(int phi) +{ + if(phi>25|| phi<-25) { + phi=10; + } + speedctrl.setDesiredSpeed(phi, phi); +} void Brobot::forward() { - //*pwmL=0.65f; // asterisk is dereferencing the pointer so - //*pwmR=0.36f; // you can access the variable at the pointers address - // also another way to dereference the pointer is: pwmR->write(0.xx) - speedctrl->setDesiredSpeed(0.65f, 0.35f); -} - -void Brobot::slow(float scale) -{ - if(scale>0.5f || scale<-0.5f) { - scale=0; - } - - *pwmL = *pwmL - scale; - *pwmR = *pwmR + scale; -} - -void Brobot::turnleft() -{ - speedctrl->setDesiredSpeed(0.48f, 0.31f); - wait(0.1); -} - -void Brobot::turnright() -{ - speedctrl->setDesiredSpeed(0.69f, 0.52f); - wait(0.1); + speedctrl.setDesiredSpeed(20, -20); // rpms } void Brobot::back() { - speedctrl->setDesiredSpeed(0.65f, 0.35f); + speedctrl.setDesiredSpeed(-15, 15); //rpms } void Brobot::stop() { - speedctrl->setDesiredSpeed(0.5f, 0.5f); + speedctrl.setDesiredSpeed(0.0f, 0.0f); } \ No newline at end of file