Mechatronics Robotics / Mbed 2 deprecated BrobotV1

These are the core files for the Robot at Team conception.

Dependencies:   mbed UniServ

Brobot.cpp@11:05d5539141c8, 2017-06-01 (annotated)

Committer:
obrie829
Date:
Thu Jun 01 08:34:27 2017 +0000
Revision:
11:05d5539141c8
Parent:
8:351b0b7b05b2
Child:
12:9a763d149f61
Child:
13:5c2a7dede65f
v10

Who changed what in which revision?

UserRevisionLine numberNew contents of line
obrie829 0:eba74e7a229b 1 /*
obrie829 0:eba74e7a229b 2 * BROBOT.cpp
obrie829 0:eba74e7a229b 3 *
obrie829 0:eba74e7a229b 4 */
obrie829 0:eba74e7a229b 5
obrie829 0:eba74e7a229b 6 #include <cmath>
obrie829 0:eba74e7a229b 7 #include "Brobot.h"
obrie829 11:05d5539141c8 8
obrie829 11:05d5539141c8 9 Brobot::Brobot(SpeedControl& speedctrl_, AnalogIn& distance_, DigitalOut& enable_, DigitalOut& bit0_, DigitalOut& bit1_, DigitalOut& bit2_, DigitalOut* led_ptr, Pixy& pixy_) :
obrie829 11:05d5539141c8 10 speedctrl(speedctrl_), distance(distance_), enable(enable_), bit0(bit0_), bit1(bit1_), bit2(bit2_), leds(led_ptr), pixy(pixy_) // assigning parameters to class variables
obrie829 11:05d5539141c8 11 {
obrie829 11:05d5539141c8 12 //initialize distance sensors
obrie829 11:05d5539141c8 13 for( int ii = 0; ii<6; ++ii)
obrie829 11:05d5539141c8 14 sensors[ii].init(&distance, &bit0, &bit1, &bit2, ii);
obrie829 11:05d5539141c8 15
obrie829 11:05d5539141c8 16 //defining the sensors for
obrie829 11:05d5539141c8 17 sensor_front.init( &distance, &bit0, &bit1, &bit2, 0); // & give the address of the object
obrie829 11:05d5539141c8 18 sensor_left.init( &distance, &bit0, &bit1, &bit2, 5);
obrie829 11:05d5539141c8 19 sensor_right.init( &distance, &bit0, &bit1, &bit2, 1);
obrie829 0:eba74e7a229b 20
obrie829 11:05d5539141c8 21 // kp ki kd min max
obrie829 11:05d5539141c8 22 //pid.setPIDValues( 0.025f, 0.00005f, 0.001f, 0.5f, -0.5f, 1000);
obrie829 11:05d5539141c8 23 pid.setPIDValues( 0.4f, 0.001f, 0.001f, 0.5f, -0.5f, 1000);
obrie829 11:05d5539141c8 24 //pixypid.setPIDValues( 0.00001f, 0.0005f, 0.00015f, 0.3f, -0.3f, 1000);
obrie829 11:05d5539141c8 25
obrie829 11:05d5539141c8 26 }
obrie829 11:05d5539141c8 27
obrie829 11:05d5539141c8 28 void Brobot::avoidObstacleAndMove(int vtrans)
obrie829 11:05d5539141c8 29 {
obrie829 11:05d5539141c8 30 float vrot = 0; // rpm
obrie829 11:05d5539141c8 31
obrie829 11:05d5539141c8 32 if(sensor_left.read() <0.3f || sensor_right.read() <0.3f) { // to avoid obstacles
obrie829 11:05d5539141c8 33
obrie829 11:05d5539141c8 34 float e = sensor_left.read() - sensor_right.read();
obrie829 11:05d5539141c8 35 float diff = pid.calc( e, 0.05f ); // error and period are arguments
obrie829 11:05d5539141c8 36
obrie829 11:05d5539141c8 37 vrot = diff*50; //turn
obrie829 0:eba74e7a229b 38
obrie829 11:05d5539141c8 39 if( vrot<= -20)
obrie829 11:05d5539141c8 40 vrot=-20;
obrie829 11:05d5539141c8 41 else if (vrot > 20)
obrie829 11:05d5539141c8 42 vrot=20;
obrie829 11:05d5539141c8 43 }
obrie829 11:05d5539141c8 44
obrie829 11:05d5539141c8 45 speedctrl.setDesiredSpeed( vtrans - vrot, -vtrans - vrot );
obrie829 11:05d5539141c8 46
obrie829 11:05d5539141c8 47 if(sensor_front.read() <=0.2f) { // when approaching normal to wall
obrie829 11:05d5539141c8 48 speedctrl.setDesiredSpeed( 0, 0 );
obrie829 11:05d5539141c8 49
obrie829 11:05d5539141c8 50 int direction=rand()%2 ; // so there is variablility in the robots path
obrie829 11:05d5539141c8 51 if(direction==0) {
obrie829 11:05d5539141c8 52 //stingray.rotate(10);
obrie829 11:05d5539141c8 53 speedctrl.setDesiredSpeed(15, 15 );
obrie829 11:05d5539141c8 54 wait(0.3);
obrie829 11:05d5539141c8 55 } else if (direction==1) {
obrie829 11:05d5539141c8 56 //stingray.rotate(-10);
obrie829 11:05d5539141c8 57 speedctrl.setDesiredSpeed( -15, -15 );
obrie829 11:05d5539141c8 58 wait(0.3);
obrie829 11:05d5539141c8 59 }
obrie829 11:05d5539141c8 60 }
obrie829 11:05d5539141c8 61 }
obrie829 11:05d5539141c8 62
obrie829 11:05d5539141c8 63 void Brobot::startLeds()
obrie829 8:351b0b7b05b2 64 {
obrie829 11:05d5539141c8 65 t1.attach( callback(this, &Brobot::ledShow), 0.05f );
obrie829 11:05d5539141c8 66 }
obrie829 11:05d5539141c8 67
obrie829 11:05d5539141c8 68 void Brobot::ledDistance()
obrie829 11:05d5539141c8 69 {
obrie829 11:05d5539141c8 70 for( int ii = 0; ii<6; ++ii)
obrie829 11:05d5539141c8 71 sensors[ii]< 0.1f ? leds[ii].write(1) : leds[ii].write(0); // an if statement in one line
obrie829 0:eba74e7a229b 72 }
obrie829 0:eba74e7a229b 73
obrie829 11:05d5539141c8 74 void Brobot::ledShow()
obrie829 11:05d5539141c8 75 {
obrie829 11:05d5539141c8 76 static int timer = 0; // static is only the first time/loop
obrie829 11:05d5539141c8 77 // for( int ii = 0; ii<6; ++ii)
obrie829 11:05d5539141c8 78 // leds[ii] = !leds[ii];
obrie829 11:05d5539141c8 79
obrie829 11:05d5539141c8 80 //quit ticker and start led distance show
obrie829 11:05d5539141c8 81 if( ++timer > 10) {
obrie829 11:05d5539141c8 82 t1.detach();
obrie829 11:05d5539141c8 83 t1.attach( callback(this, &Brobot::ledDistance), 0.01f );
obrie829 11:05d5539141c8 84 }
obrie829 11:05d5539141c8 85 }
obrie829 11:05d5539141c8 86
obrie829 11:05d5539141c8 87 bool Brobot::foundGreenBrick()
obrie829 0:eba74e7a229b 88 {
obrie829 11:05d5539141c8 89 if( pixy.objectDetected() ) {
obrie829 11:05d5539141c8 90 if( pixy.getSignature() == 1) { // 1 is the green brick
obrie829 11:05d5539141c8 91 return true;
obrie829 11:05d5539141c8 92 }
obrie829 0:eba74e7a229b 93 }
obrie829 11:05d5539141c8 94 return false;
obrie829 11:05d5539141c8 95 }
obrie829 0:eba74e7a229b 96
obrie829 11:05d5539141c8 97 bool Brobot::foundHome()
obrie829 11:05d5539141c8 98 {
obrie829 11:05d5539141c8 99 if( pixy.objectDetected() ) {
obrie829 11:05d5539141c8 100 if( pixy.getSignature() == 2) { // 2 is the home
obrie829 11:05d5539141c8 101 return true;
obrie829 11:05d5539141c8 102 }
obrie829 11:05d5539141c8 103 }
obrie829 11:05d5539141c8 104 return false;
obrie829 0:eba74e7a229b 105 }
obrie829 0:eba74e7a229b 106
obrie829 11:05d5539141c8 107 void Brobot::approachBrick()
obrie829 11:05d5539141c8 108 {
obrie829 11:05d5539141c8 109 //insert Crystal's code
obrie829 11:05d5539141c8 110
obrie829 11:05d5539141c8 111 float e = 160-pixy.getX();
obrie829 11:05d5539141c8 112 float diff = pixypid.calc( e, 0.005f );
obrie829 11:05d5539141c8 113 while( pixy.getY() <= 200) { // in [ixels
obrie829 11:05d5539141c8 114 speedctrl.setDesiredSpeed( 8 - diff, 8 - diff) ; // 8 rpms
obrie829 11:05d5539141c8 115 }
obrie829 11:05d5539141c8 116 // function assumes Brick is in open grips
obrie829 11:05d5539141c8 117
obrie829 11:05d5539141c8 118 }
obrie829 11:05d5539141c8 119
obrie829 11:05d5539141c8 120 void Brobot::closeGrip()
obrie829 8:351b0b7b05b2 121 {
obrie829 11:05d5539141c8 122 //insert Paul's code here
obrie829 11:05d5539141c8 123 //create Servo type object (ex. servMotor) in Brobot.h
obrie829 11:05d5539141c8 124 }
obrie829 11:05d5539141c8 125
obrie829 11:05d5539141c8 126 void Brobot::openGrip()
obrie829 11:05d5539141c8 127 {
obrie829 11:05d5539141c8 128 //insert adjusted Paul's code here
obrie829 11:05d5539141c8 129 //create Servo type object (ex. servMotor) in Brobot.h
obrie829 8:351b0b7b05b2 130 }
obrie829 11:05d5539141c8 131
obrie829 11:05d5539141c8 132
obrie829 11:05d5539141c8 133 void Brobot::rotate(int phi)
obrie829 11:05d5539141c8 134 {
obrie829 11:05d5539141c8 135 if(phi>25|| phi<-25) {
obrie829 11:05d5539141c8 136 phi=10;
obrie829 11:05d5539141c8 137 }
obrie829 11:05d5539141c8 138 speedctrl.setDesiredSpeed(phi, phi);
obrie829 11:05d5539141c8 139 }
obrie829 8:351b0b7b05b2 140
obrie829 0:eba74e7a229b 141 void Brobot::forward()
obrie829 0:eba74e7a229b 142 {
obrie829 11:05d5539141c8 143 speedctrl.setDesiredSpeed(20, -20); // rpms
obrie829 0:eba74e7a229b 144 }
obrie829 0:eba74e7a229b 145
obrie829 0:eba74e7a229b 146 void Brobot::turnleft()
obrie829 0:eba74e7a229b 147 {
obrie829 11:05d5539141c8 148 speedctrl.setDesiredSpeed(0.48f, 0.31f); // needs tuning
obrie829 0:eba74e7a229b 149 wait(0.1);
obrie829 0:eba74e7a229b 150 }
obrie829 0:eba74e7a229b 151
obrie829 0:eba74e7a229b 152 void Brobot::turnright()
obrie829 0:eba74e7a229b 153 {
obrie829 11:05d5539141c8 154 speedctrl.setDesiredSpeed(0.69f, 0.52f); // needs tuning
obrie829 0:eba74e7a229b 155 wait(0.1);
obrie829 0:eba74e7a229b 156 }
obrie829 0:eba74e7a229b 157
obrie829 0:eba74e7a229b 158 void Brobot::back()
obrie829 0:eba74e7a229b 159 {
obrie829 11:05d5539141c8 160 speedctrl.setDesiredSpeed(-15, 15); //rpms
obrie829 0:eba74e7a229b 161 }
obrie829 0:eba74e7a229b 162
obrie829 0:eba74e7a229b 163 void Brobot::stop()
obrie829 0:eba74e7a229b 164 {
obrie829 11:05d5539141c8 165 speedctrl.setDesiredSpeed(0.0f, 0.0f);
obrie829 0:eba74e7a229b 166 }