Diff: Brobot.cpp

Revision:

15:4efc66de795a

Parent:

8:351b0b7b05b2

Child:

17:ec52258b9472

--- 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