Chandan Siyag / Mbed 2 deprecated AlooMBED

3rd year group project. Electronic and Electrical Engineering. Heriot-Watt University. This is the code for the mbed for the Automatic Little Object Organiser (ALOO).

Dependencies:   MCP23017 TCS3472_I2C WattBob_TextLCD mbed

fpga.cpp@21:60c4fe94d79c, 2015-11-25 (annotated)

Committer:
dreamselec
Date:
Wed Nov 25 19:21:12 2015 +0000
Revision:
21:60c4fe94d79c
Parent:
10:16ba52f8e025
Child:
22:993821a4c396
Optimised the code so, bottom servo is fast enough to sort the blocks. Code starts moving the servo after it detects block, checks the colour, then the size, if size and colour don't match then it moves the servo back to non haz.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dreamselec 6:98fe30430194 1 //
dreamselec 6:98fe30430194 2 // fpga.cpp
dreamselec 6:98fe30430194 3 // Created by Chandan Siyag on 14/11/2015.
dreamselec 6:98fe30430194 4
dreamselec 3:843b830ee8bd 5 #include "fpga.h"
dreamselec 3:843b830ee8bd 6 #include "mbed.h"
dreamselec 6:98fe30430194 7
dreamselec 6:98fe30430194 8 #define CLK() bbClk.write(1); wait(0.000001); bbClk.write(0); wait(0.000001);
dreamselec 3:843b830ee8bd 9
dreamselec 6:98fe30430194 10 DigitalOut bbClk(p11);
dreamselec 6:98fe30430194 11 DigitalOut bbRst(p12);
dreamselec 6:98fe30430194 12 DigitalOut bbOut(p13);
dreamselec 6:98fe30430194 13 DigitalIn bbIn(p14);
dreamselec 6:98fe30430194 14 int brkBbm_1, brkBbm_2, brkBbm_3, brkBbm_4, brkBbm_5, brkBbm_6, brkBbm_7, brkBbm_8;
dreamselec 6:98fe30430194 15
dreamselec 6:98fe30430194 16 FPGA::FPGA(){
dreamselec 6:98fe30430194 17
dreamselec 6:98fe30430194 18 }
dreamselec 6:98fe30430194 19
dreamselec 6:98fe30430194 20 FPGA::~FPGA(){
dreamselec 6:98fe30430194 21
dreamselec 6:98fe30430194 22 }
dreamselec 3:843b830ee8bd 23
dreamselec 3:843b830ee8bd 24 /** Function that checks first IR sensor to detect if block in shoot
dreamselec 3:843b830ee8bd 25 @return 1 if sensor beam broken, 0 otherwise
dreamselec 6:98fe30430194 26 */
dreamselec 6:98fe30430194 27 int FPGA::checkForBlock(){
dreamselec 6:98fe30430194 28 return this->getBeamValue(1);
dreamselec 3:843b830ee8bd 29 }
dreamselec 3:843b830ee8bd 30
dreamselec 3:843b830ee8bd 31 /** Function that checks if block is also breaking the second IR sensor.
dreamselec 6:98fe30430194 32 @result 1 if it's large, 0 otherwise
dreamselec 6:98fe30430194 33 */
dreamselec 6:98fe30430194 34 int FPGA::checkForSize(){
dreamselec 6:98fe30430194 35 return this->getBeamValue(2);
dreamselec 3:843b830ee8bd 36 }
dreamselec 3:843b830ee8bd 37
dreamselec 3:843b830ee8bd 38 /** Moves the stopping servo in the "hopper" to stop blocks from falling
dreamselec 3:843b830ee8bd 39 @param position: TBD
dreamselec 6:98fe30430194 40 */
dreamselec 6:98fe30430194 41 void FPGA::moveStoppingServo(StoppingServoPositions position) {
dreamselec 6:98fe30430194 42 if (position == Stop){
dreamselec 6:98fe30430194 43 bbRst.write(0);
dreamselec 6:98fe30430194 44 CLK();
dreamselec 6:98fe30430194 45 bbRst.write(1);
dreamselec 6:98fe30430194 46 CLK(); //Reset finish
dreamselec 6:98fe30430194 47 bbOut.write(1);
dreamselec 6:98fe30430194 48 CLK();
dreamselec 6:98fe30430194 49 bbOut.write(1);
dreamselec 6:98fe30430194 50 CLK();
dreamselec 6:98fe30430194 51 bbOut.write(0);
dreamselec 6:98fe30430194 52 CLK(); // Address finish
dreamselec 6:98fe30430194 53
dreamselec 6:98fe30430194 54 bbOut.write(0);
dreamselec 6:98fe30430194 55 CLK();
dreamselec 6:98fe30430194 56 bbOut.write(0);
dreamselec 6:98fe30430194 57 CLK();
dreamselec 6:98fe30430194 58 bbOut.write(0);
dreamselec 6:98fe30430194 59 CLK();
dreamselec 6:98fe30430194 60 bbOut.write(0);
dreamselec 6:98fe30430194 61 CLK();
dreamselec 6:98fe30430194 62 bbOut.write(0);
dreamselec 6:98fe30430194 63 CLK();
dreamselec 6:98fe30430194 64 bbOut.write(1);
dreamselec 6:98fe30430194 65 CLK();
dreamselec 6:98fe30430194 66 bbOut.write(1);
dreamselec 6:98fe30430194 67 CLK();
dreamselec 6:98fe30430194 68 bbOut.write(0);
dreamselec 6:98fe30430194 69 CLK(); // Position finish
dreamselec 6:98fe30430194 70
dreamselec 6:98fe30430194 71 bbOut.write(1);
dreamselec 6:98fe30430194 72 CLK();
dreamselec 6:98fe30430194 73 bbOut.write(0);
dreamselec 6:98fe30430194 74 CLK();
dreamselec 6:98fe30430194 75 bbOut.write(0);
dreamselec 6:98fe30430194 76 CLK();
dreamselec 6:98fe30430194 77 bbOut.write(0);
dreamselec 6:98fe30430194 78 CLK();
dreamselec 6:98fe30430194 79 bbOut.write(0);
dreamselec 6:98fe30430194 80 CLK();
dreamselec 6:98fe30430194 81 bbOut.write(0);
dreamselec 6:98fe30430194 82 CLK();
dreamselec 6:98fe30430194 83 bbOut.write(0);
dreamselec 6:98fe30430194 84 CLK();
dreamselec 6:98fe30430194 85 bbOut.write(0);
dreamselec 6:98fe30430194 86 CLK(); // Number finish
dreamselec 6:98fe30430194 87 bbOut.write(0);
dreamselec 6:98fe30430194 88 }else if (position == Go){
dreamselec 6:98fe30430194 89 bbRst.write(0);
dreamselec 6:98fe30430194 90 CLK();
dreamselec 6:98fe30430194 91 bbRst.write(1);
dreamselec 6:98fe30430194 92 CLK(); //Reset finish
dreamselec 6:98fe30430194 93 bbOut.write(1);
dreamselec 6:98fe30430194 94 CLK();
dreamselec 6:98fe30430194 95 bbOut.write(1);
dreamselec 6:98fe30430194 96 CLK();
dreamselec 6:98fe30430194 97 bbOut.write(0);
dreamselec 6:98fe30430194 98 CLK(); // Address finish
dreamselec 6:98fe30430194 99
dreamselec 6:98fe30430194 100 bbOut.write(1);
dreamselec 6:98fe30430194 101 CLK();
dreamselec 6:98fe30430194 102 bbOut.write(0);
dreamselec 6:98fe30430194 103 CLK();
dreamselec 6:98fe30430194 104 bbOut.write(0);
dreamselec 6:98fe30430194 105 CLK();
dreamselec 6:98fe30430194 106 bbOut.write(0);
dreamselec 6:98fe30430194 107 CLK();
dreamselec 6:98fe30430194 108 bbOut.write(0);
dreamselec 6:98fe30430194 109 CLK();
dreamselec 6:98fe30430194 110 bbOut.write(0);
dreamselec 6:98fe30430194 111 CLK();
dreamselec 6:98fe30430194 112 bbOut.write(0);
dreamselec 6:98fe30430194 113 CLK();
dreamselec 6:98fe30430194 114 bbOut.write(0);
dreamselec 6:98fe30430194 115 CLK(); //Position finish
dreamselec 6:98fe30430194 116
dreamselec 6:98fe30430194 117 bbOut.write(1);
dreamselec 6:98fe30430194 118 CLK();
dreamselec 6:98fe30430194 119 bbOut.write(0);
dreamselec 6:98fe30430194 120 CLK();
dreamselec 6:98fe30430194 121 bbOut.write(0);
dreamselec 6:98fe30430194 122 CLK();
dreamselec 6:98fe30430194 123 bbOut.write(0);
dreamselec 6:98fe30430194 124 CLK();
dreamselec 6:98fe30430194 125 bbOut.write(0);
dreamselec 6:98fe30430194 126 CLK();
dreamselec 6:98fe30430194 127 bbOut.write(0);
dreamselec 6:98fe30430194 128 CLK();
dreamselec 6:98fe30430194 129 bbOut.write(0);
dreamselec 6:98fe30430194 130 CLK();
dreamselec 6:98fe30430194 131 bbOut.write(0);
dreamselec 6:98fe30430194 132 CLK(); //Number finish
dreamselec 6:98fe30430194 133 bbOut.write(0);
dreamselec 6:98fe30430194 134 }
dreamselec 10:16ba52f8e025 135 this->stoppingServoPosition = position;
dreamselec 3:843b830ee8bd 136 }
dreamselec 3:843b830ee8bd 137
dreamselec 3:843b830ee8bd 138 /** Moves the servoe which decides which basket block should go into i.e. bad/good
dreamselec 3:843b830ee8bd 139 @param positon: TBD
dreamselec 6:98fe30430194 140 */
dreamselec 6:98fe30430194 141 void FPGA::moveSortingServo(SortingServoPositions position){
dreamselec 10:16ba52f8e025 142 if (position == Haz){
dreamselec 10:16ba52f8e025 143 // HAZ = 23
dreamselec 10:16ba52f8e025 144 bbRst.write(0);
dreamselec 10:16ba52f8e025 145 CLK();
dreamselec 10:16ba52f8e025 146 bbRst.write(1);
dreamselec 10:16ba52f8e025 147 CLK(); //Reset finish
dreamselec 10:16ba52f8e025 148 bbOut.write(1);
dreamselec 10:16ba52f8e025 149 CLK();
dreamselec 10:16ba52f8e025 150 bbOut.write(1);
dreamselec 10:16ba52f8e025 151 CLK();
dreamselec 10:16ba52f8e025 152 bbOut.write(0);
dreamselec 10:16ba52f8e025 153 CLK(); // Address finish
dreamselec 10:16ba52f8e025 154 bbOut.write(1);
dreamselec 10:16ba52f8e025 155 CLK();
dreamselec 10:16ba52f8e025 156 bbOut.write(1);
dreamselec 10:16ba52f8e025 157 CLK();
dreamselec 10:16ba52f8e025 158 bbOut.write(1);
dreamselec 10:16ba52f8e025 159 CLK();
dreamselec 10:16ba52f8e025 160 bbOut.write(0);
dreamselec 10:16ba52f8e025 161 CLK();
dreamselec 10:16ba52f8e025 162 bbOut.write(1);
dreamselec 10:16ba52f8e025 163 CLK();
dreamselec 10:16ba52f8e025 164 bbOut.write(0);
dreamselec 10:16ba52f8e025 165 CLK();
dreamselec 10:16ba52f8e025 166 bbOut.write(0);
dreamselec 10:16ba52f8e025 167 CLK();
dreamselec 10:16ba52f8e025 168 bbOut.write(0);
dreamselec 10:16ba52f8e025 169 CLK(); //Position finish
dreamselec 10:16ba52f8e025 170 bbOut.write(0);
dreamselec 10:16ba52f8e025 171 CLK();
dreamselec 10:16ba52f8e025 172 bbOut.write(1);
dreamselec 10:16ba52f8e025 173 CLK();
dreamselec 10:16ba52f8e025 174 bbOut.write(0);
dreamselec 10:16ba52f8e025 175 CLK();
dreamselec 10:16ba52f8e025 176 bbOut.write(0);
dreamselec 10:16ba52f8e025 177 CLK();
dreamselec 10:16ba52f8e025 178 bbOut.write(0);
dreamselec 10:16ba52f8e025 179 CLK();
dreamselec 10:16ba52f8e025 180 bbOut.write(0);
dreamselec 10:16ba52f8e025 181 CLK();
dreamselec 10:16ba52f8e025 182 bbOut.write(0);
dreamselec 10:16ba52f8e025 183 CLK();
dreamselec 10:16ba52f8e025 184 bbOut.write(0);
dreamselec 10:16ba52f8e025 185 CLK(); //Number finish
dreamselec 10:16ba52f8e025 186 bbOut.write(0);
dreamselec 10:16ba52f8e025 187 }else if (position == NonHaz){
dreamselec 21:60c4fe94d79c 188 // NON-HAZ = 73
dreamselec 10:16ba52f8e025 189 bbRst.write(0);
dreamselec 10:16ba52f8e025 190 CLK();
dreamselec 10:16ba52f8e025 191 bbRst.write(1);
dreamselec 10:16ba52f8e025 192 CLK(); //Reset finish
dreamselec 10:16ba52f8e025 193 bbOut.write(1);
dreamselec 10:16ba52f8e025 194 CLK();
dreamselec 10:16ba52f8e025 195 bbOut.write(1);
dreamselec 10:16ba52f8e025 196 CLK();
dreamselec 10:16ba52f8e025 197 bbOut.write(0);
dreamselec 10:16ba52f8e025 198 CLK(); // Address finish
dreamselec 10:16ba52f8e025 199 bbOut.write(0);
dreamselec 10:16ba52f8e025 200 CLK();
dreamselec 10:16ba52f8e025 201 bbOut.write(0);
dreamselec 10:16ba52f8e025 202 CLK();
dreamselec 21:60c4fe94d79c 203 bbOut.write(1);
dreamselec 21:60c4fe94d79c 204 CLK();
dreamselec 21:60c4fe94d79c 205 bbOut.write(1);
dreamselec 21:60c4fe94d79c 206 CLK();
dreamselec 21:60c4fe94d79c 207 bbOut.write(1);
dreamselec 10:16ba52f8e025 208 CLK();
dreamselec 10:16ba52f8e025 209 bbOut.write(1);
dreamselec 10:16ba52f8e025 210 CLK();
dreamselec 10:16ba52f8e025 211 bbOut.write(0);
dreamselec 10:16ba52f8e025 212 CLK();
dreamselec 10:16ba52f8e025 213 bbOut.write(0);
dreamselec 10:16ba52f8e025 214 CLK(); //Position finish
dreamselec 10:16ba52f8e025 215 bbOut.write(0);
dreamselec 10:16ba52f8e025 216 CLK();
dreamselec 10:16ba52f8e025 217 bbOut.write(1);
dreamselec 10:16ba52f8e025 218 CLK();
dreamselec 10:16ba52f8e025 219 bbOut.write(0);
dreamselec 10:16ba52f8e025 220 CLK();
dreamselec 10:16ba52f8e025 221 bbOut.write(0);
dreamselec 10:16ba52f8e025 222 CLK();
dreamselec 10:16ba52f8e025 223 bbOut.write(0);
dreamselec 10:16ba52f8e025 224 CLK();
dreamselec 10:16ba52f8e025 225 bbOut.write(0);
dreamselec 10:16ba52f8e025 226 CLK();
dreamselec 10:16ba52f8e025 227 bbOut.write(0);
dreamselec 10:16ba52f8e025 228 CLK();
dreamselec 10:16ba52f8e025 229 bbOut.write(0);
dreamselec 10:16ba52f8e025 230 CLK(); //Number finish
dreamselec 10:16ba52f8e025 231 bbOut.write(0);
dreamselec 6:98fe30430194 232 }
dreamselec 10:16ba52f8e025 233 this->sortingServoPosition = position;
dreamselec 10:16ba52f8e025 234 }
dreamselec 10:16ba52f8e025 235
dreamselec 10:16ba52f8e025 236 void FPGA::toggleStoppingServo(){
dreamselec 10:16ba52f8e025 237 if (this->stoppingServoPosition == Go)
dreamselec 10:16ba52f8e025 238 this->moveStoppingServo(Stop);
dreamselec 10:16ba52f8e025 239 else if (this->stoppingServoPosition == Stop)
dreamselec 10:16ba52f8e025 240 this->moveStoppingServo(Go);
dreamselec 10:16ba52f8e025 241 }
dreamselec 10:16ba52f8e025 242
dreamselec 10:16ba52f8e025 243 void FPGA::toggleSortingServo() {
dreamselec 10:16ba52f8e025 244 if (this->sortingServoPosition == Haz)
dreamselec 10:16ba52f8e025 245 this->moveSortingServo(NonHaz);
dreamselec 10:16ba52f8e025 246 else if (this->sortingServoPosition == NonHaz)
dreamselec 10:16ba52f8e025 247 this->moveSortingServo(Haz);
dreamselec 3:843b830ee8bd 248 }
dreamselec 3:843b830ee8bd 249
dreamselec 3:843b830ee8bd 250 /** Reset the stopping servo to default posiotn i.e. not letting blcoks fall.
dreamselec 6:98fe30430194 251 */
dreamselec 6:98fe30430194 252 void FPGA::resetStoppingServo(){
dreamselec 6:98fe30430194 253
dreamselec 3:843b830ee8bd 254 }
dreamselec 3:843b830ee8bd 255 /** Reset the sorting servo to default posiont i.e. falling blocks go in haz-bin
dreamselec 6:98fe30430194 256 */
dreamselec 6:98fe30430194 257 void FPGA::resetSortingServo(){
dreamselec 6:98fe30430194 258
dreamselec 3:843b830ee8bd 259 }
dreamselec 3:843b830ee8bd 260 /** Reset both servos, simply calls their respective reset methods.
dreamselec 6:98fe30430194 261 */
dreamselec 6:98fe30430194 262 void FPGA::resetAllServos(){
dreamselec 6:98fe30430194 263 resetSortingServo();
dreamselec 6:98fe30430194 264 resetStoppingServo();
dreamselec 6:98fe30430194 265 }
dreamselec 6:98fe30430194 266
dreamselec 6:98fe30430194 267 int FPGA::getBeamValue(int beamNumber){
dreamselec 6:98fe30430194 268 // Return 0 or 1 value for break beam
dreamselec 6:98fe30430194 269 bbRst.write(0);
dreamselec 6:98fe30430194 270 CLK();
dreamselec 6:98fe30430194 271 bbRst.write(1);
dreamselec 6:98fe30430194 272 CLK(); //Reset finish
dreamselec 6:98fe30430194 273 bbOut.write(1);
dreamselec 6:98fe30430194 274 CLK();
dreamselec 6:98fe30430194 275 bbOut.write(0);
dreamselec 6:98fe30430194 276 CLK();
dreamselec 6:98fe30430194 277 bbOut.write(1);
dreamselec 6:98fe30430194 278 CLK(); //Address finish
dreamselec 6:98fe30430194 279 CLK();
dreamselec 6:98fe30430194 280 brkBbm_1 = bbIn.read();
dreamselec 6:98fe30430194 281 CLK();
dreamselec 6:98fe30430194 282 brkBbm_2 = bbIn.read();
dreamselec 6:98fe30430194 283 CLK();
dreamselec 6:98fe30430194 284 brkBbm_3 = bbIn.read();
dreamselec 6:98fe30430194 285 CLK();
dreamselec 6:98fe30430194 286 brkBbm_4 = bbIn.read();
dreamselec 6:98fe30430194 287 CLK();
dreamselec 6:98fe30430194 288 brkBbm_5 = bbIn.read();
dreamselec 6:98fe30430194 289 CLK();
dreamselec 6:98fe30430194 290 brkBbm_6 = bbIn.read();
dreamselec 6:98fe30430194 291 CLK();
dreamselec 6:98fe30430194 292 brkBbm_7 = bbIn.read();
dreamselec 6:98fe30430194 293 CLK();
dreamselec 6:98fe30430194 294 brkBbm_8 = bbIn.read();
dreamselec 6:98fe30430194 295 CLK(); //Break beam data recieved and saved
dreamselec 6:98fe30430194 296 bbOut.write(0);
dreamselec 6:98fe30430194 297
dreamselec 6:98fe30430194 298 if (beamNumber == 1) {
dreamselec 6:98fe30430194 299 return brkBbm_1;
dreamselec 6:98fe30430194 300 } else if (beamNumber == 2) {
dreamselec 6:98fe30430194 301 return brkBbm_2;
dreamselec 6:98fe30430194 302 }
dreamselec 6:98fe30430194 303 return -1;
dreamselec 6:98fe30430194 304 }
dreamselec 6:98fe30430194 305
dreamselec 6:98fe30430194 306 void FPGA::moveServo(int servoNumber, int position){
dreamselec 6:98fe30430194 307
dreamselec 6:98fe30430194 308 }