Mechatronics Robotics
/
BrobotV1
These are the core files for the Robot at Team conception.
Brobot.cpp@13:5c2a7dede65f, 2017-06-01 (annotated)
- Committer:
- huynh270
- Date:
- Thu Jun 01 09:46:53 2017 +0000
- Revision:
- 13:5c2a7dede65f
- Parent:
- 11:05d5539141c8
- Child:
- 14:6a3eb8e8a75e
restructure main and added to approach brick
Who changed what in which revision?
User | Revision | Line number | New 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); |
huynh270 | 13:5c2a7dede65f | 24 | pixypid.setPIDValues( 0.001f, 0.0000f, 0.000000f, 0.5f, -0.5f, 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 | } |