![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
These are the core files for the Robot at Team conception.
Brobot.cpp
- Committer:
- obrie829
- Date:
- 2017-06-05
- Revision:
- 15:4efc66de795a
- Parent:
- 8:351b0b7b05b2
- Child:
- 17:ec52258b9472
File content as of revision 15:4efc66de795a:
/* * BROBOT.cpp * */ #include <cmath> #include "Brobot.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); } 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; } 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() { 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 ); } } bool Brobot::foundGreenBrick() { if( pixy.objectDetected() ) { if( pixy.getSignature() == 1) { // 1 is the green brick return true; } } return false; } bool Brobot::foundHome() { if( pixy.objectDetected() ) { if( pixy.getSignature() == 2) { // 2 is the home return true; } } return false; } 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 } bool Brobot::approachHome() { 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() { speedctrl.setDesiredSpeed(20, -20); // rpms } void Brobot::back() { speedctrl.setDesiredSpeed(-15, 15); //rpms } void Brobot::stop() { speedctrl.setDesiredSpeed(0.0f, 0.0f); }