FinalRobotCode/RobotMain/TubeProcessing.cpp at master · RBE2001Group8/FinalRobotCode · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
/** Subsystem that gets available storage/supply tubes to the given
  * reactor
  * @author Hans Johnson
  * @date Oct. 2015
  **/

#include "TubeProcessing.h"

/* Constructor */
TubeProcessing::TubeProcessing() {}

int TubeProcessing::getStorageTube(char bitmask, bool sideA) {
  int i;
  if (sideA) {
    for (i = 0; i<4; i++) {
      //Looking for a zero, XOR with 1 will be 1/true if there is a zero there, 0 if there is a 1 in that position
      if (((~bitmask)>>i)&0x01) {
        break;
      }
    }
  } else {
    for (i = 3; i>=0; i--) {
      //Looking for a zero, XOR with 1 will be 1/true if a zero is there, 0 if a 1 is in that position
      if (((~bitmask)>>i)&0x01) {
        break;
      }
    }
  }
  //(Code assumes first storage tube is number 1)
  return i+1;
}

int TubeProcessing::getFreshRodTube(char bitmask, bool sideA) {
  int i;
  if (sideA) {
    for (i = 0; i<4; i++) {
      //Looking for a zero, AND with 1 will be 1/true if there is a 1, 0 otherwise
      if ((bitmask>>i)&0x01) {
        break;
      }
    }
  } else {
    for (i = 3; i>=0; i--) {
      //Looking for a zero, AND with 1 will be 1/true if there is a 1, 0 otherwise
      if ((bitmask>>i)&0x01) {
        break;
      }
    }
  }
  return i+1; //(Code assumes first storage tube is number 1)
}