![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Code to run the microcontrollers on the R5 competition bot
Dependencies: LineSensors mbed
navcontroller.cpp@15:150ec9448efc, 2015-04-18 (annotated)
- Committer:
- Hypna
- Date:
- Sat Apr 18 02:54:55 2015 +0000
- Revision:
- 15:150ec9448efc
experiment
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Hypna | 15:150ec9448efc | 1 | #include <cstdlib> |
Hypna | 15:150ec9448efc | 2 | #include <cstring> |
Hypna | 15:150ec9448efc | 3 | #include <cstdio> |
Hypna | 15:150ec9448efc | 4 | #include <sstream> |
Hypna | 15:150ec9448efc | 5 | #include <string> |
Hypna | 15:150ec9448efc | 6 | #include <iostream> |
Hypna | 15:150ec9448efc | 7 | #include <vector> |
Hypna | 15:150ec9448efc | 8 | #include "navcontroller.h" |
Hypna | 15:150ec9448efc | 9 | #include <opencv2/highgui/highgui.hpp> |
Hypna | 15:150ec9448efc | 10 | #include <opencv2/imgproc/imgproc.hpp> |
Hypna | 15:150ec9448efc | 11 | #include <tesseract/baseapi.h> |
Hypna | 15:150ec9448efc | 12 | #include <unistd> |
Hypna | 15:150ec9448efc | 13 | extern "C" { |
Hypna | 15:150ec9448efc | 14 | #include <wiringX.h> |
Hypna | 15:150ec9448efc | 15 | } |
Hypna | 15:150ec9448efc | 16 | |
Hypna | 15:150ec9448efc | 17 | NavController::NavController(int mapSize) : map(mapSize), orient(NORTH) |
Hypna | 15:150ec9448efc | 18 | { |
Hypna | 15:150ec9448efc | 19 | initIO(); |
Hypna | 15:150ec9448efc | 20 | end.x = mapSize-1; |
Hypna | 15:150ec9448efc | 21 | end.y = mapSize-1; |
Hypna | 15:150ec9448efc | 22 | } |
Hypna | 15:150ec9448efc | 23 | |
Hypna | 15:150ec9448efc | 24 | void NavController::explore() |
Hypna | 15:150ec9448efc | 25 | { |
Hypna | 15:150ec9448efc | 26 | typename ::direction next; |
Hypna | 15:150ec9448efc | 27 | typename ::position newPos; |
Hypna | 15:150ec9448efc | 28 | |
Hypna | 15:150ec9448efc | 29 | while(map.getPosition() != end) |
Hypna | 15:150ec9448efc | 30 | { |
Hypna | 15:150ec9448efc | 31 | updateMap(); |
Hypna | 15:150ec9448efc | 32 | next = nextExploreMove(); |
Hypna | 15:150ec9448efc | 33 | sendMove(next); |
Hypna | 15:150ec9448efc | 34 | newPos = map.getPosition(); |
Hypna | 15:150ec9448efc | 35 | |
Hypna | 15:150ec9448efc | 36 | switch(next) |
Hypna | 15:150ec9448efc | 37 | { |
Hypna | 15:150ec9448efc | 38 | case NORTH : newPos.y++; |
Hypna | 15:150ec9448efc | 39 | break; |
Hypna | 15:150ec9448efc | 40 | case SOUTH : newPos.y--; |
Hypna | 15:150ec9448efc | 41 | break; |
Hypna | 15:150ec9448efc | 42 | case EAST : newPos.x++; |
Hypna | 15:150ec9448efc | 43 | break; |
Hypna | 15:150ec9448efc | 44 | case WEST : newPos.x--; |
Hypna | 15:150ec9448efc | 45 | } |
Hypna | 15:150ec9448efc | 46 | |
Hypna | 15:150ec9448efc | 47 | map.setPosition(newPos); |
Hypna | 15:150ec9448efc | 48 | } |
Hypna | 15:150ec9448efc | 49 | |
Hypna | 15:150ec9448efc | 50 | saveMap(); |
Hypna | 15:150ec9448efc | 51 | } |
Hypna | 15:150ec9448efc | 52 | |
Hypna | 15:150ec9448efc | 53 | void NavController::goBack() |
Hypna | 15:150ec9448efc | 54 | { |
Hypna | 15:150ec9448efc | 55 | direction next; |
Hypna | 15:150ec9448efc | 56 | typename ::position newPos; |
Hypna | 15:150ec9448efc | 57 | map.setPosition(map.getStart()); |
Hypna | 15:150ec9448efc | 58 | |
Hypna | 15:150ec9448efc | 59 | stack<direction> temp; |
Hypna | 15:150ec9448efc | 60 | |
Hypna | 15:150ec9448efc | 61 | while(!path.empty()) |
Hypna | 15:150ec9448efc | 62 | { |
Hypna | 15:150ec9448efc | 63 | temp.push(path.top()); |
Hypna | 15:150ec9448efc | 64 | path.pop(); |
Hypna | 15:150ec9448efc | 65 | } |
Hypna | 15:150ec9448efc | 66 | |
Hypna | 15:150ec9448efc | 67 | path = temp; |
Hypna | 15:150ec9448efc | 68 | |
Hypna | 15:150ec9448efc | 69 | |
Hypna | 15:150ec9448efc | 70 | while(map.getPosition() != end) |
Hypna | 15:150ec9448efc | 71 | { |
Hypna | 15:150ec9448efc | 72 | next = path.top(); |
Hypna | 15:150ec9448efc | 73 | path.pop(); |
Hypna | 15:150ec9448efc | 74 | sendMove(next); |
Hypna | 15:150ec9448efc | 75 | newPos = map.getPosition(); |
Hypna | 15:150ec9448efc | 76 | |
Hypna | 15:150ec9448efc | 77 | switch(next) |
Hypna | 15:150ec9448efc | 78 | { |
Hypna | 15:150ec9448efc | 79 | case NORTH : newPos.y++; |
Hypna | 15:150ec9448efc | 80 | break; |
Hypna | 15:150ec9448efc | 81 | case SOUTH : newPos.y--; |
Hypna | 15:150ec9448efc | 82 | break; |
Hypna | 15:150ec9448efc | 83 | case EAST : newPos.x++; |
Hypna | 15:150ec9448efc | 84 | break; |
Hypna | 15:150ec9448efc | 85 | case WEST : newPos.x--; |
Hypna | 15:150ec9448efc | 86 | } |
Hypna | 15:150ec9448efc | 87 | |
Hypna | 15:150ec9448efc | 88 | map.setPosition(newPos); |
Hypna | 15:150ec9448efc | 89 | } |
Hypna | 15:150ec9448efc | 90 | } |
Hypna | 15:150ec9448efc | 91 | |
Hypna | 15:150ec9448efc | 92 | direction NavController::nextExploreMove() |
Hypna | 15:150ec9448efc | 93 | { |
Hypna | 15:150ec9448efc | 94 | typename ::position current = map.getPosition(); |
Hypna | 15:150ec9448efc | 95 | typename ::position dest = map.getFinish(); |
Hypna | 15:150ec9448efc | 96 | |
Hypna | 15:150ec9448efc | 97 | int northWgt = dest.y - current.y; |
Hypna | 15:150ec9448efc | 98 | int southWgt = current.y; |
Hypna | 15:150ec9448efc | 99 | int eastWgt = current.x; |
Hypna | 15:150ec9448efc | 100 | int westWgt = current.x - dest.x; |
Hypna | 15:150ec9448efc | 101 | |
Hypna | 15:150ec9448efc | 102 | direction nextMove; |
Hypna | 15:150ec9448efc | 103 | |
Hypna | 15:150ec9448efc | 104 | if(map.getVisited(NORTH) || map.getCurWall(NORTH) == CLOSED) |
Hypna | 15:150ec9448efc | 105 | northWgt = 0; |
Hypna | 15:150ec9448efc | 106 | |
Hypna | 15:150ec9448efc | 107 | if(map.getVisited(SOUTH) || map.getCurWall(SOUTH) == CLOSED) |
Hypna | 15:150ec9448efc | 108 | southWgt = 0; |
Hypna | 15:150ec9448efc | 109 | |
Hypna | 15:150ec9448efc | 110 | if(map.getVisited(EAST) || map.getCurWall(EAST) == CLOSED) |
Hypna | 15:150ec9448efc | 111 | eastWgt = 0; |
Hypna | 15:150ec9448efc | 112 | |
Hypna | 15:150ec9448efc | 113 | if(map.getVisited(WEST) || map.getCurWall(WEST) == CLOSED) |
Hypna | 15:150ec9448efc | 114 | westWgt = 0; |
Hypna | 15:150ec9448efc | 115 | |
Hypna | 15:150ec9448efc | 116 | if(northWgt != 0 || westWgt != 0) |
Hypna | 15:150ec9448efc | 117 | { |
Hypna | 15:150ec9448efc | 118 | if(northWgt > westWgt) |
Hypna | 15:150ec9448efc | 119 | nextMove = NORTH; |
Hypna | 15:150ec9448efc | 120 | else |
Hypna | 15:150ec9448efc | 121 | nextMove = WEST; |
Hypna | 15:150ec9448efc | 122 | |
Hypna | 15:150ec9448efc | 123 | path.push(nextMove); |
Hypna | 15:150ec9448efc | 124 | } |
Hypna | 15:150ec9448efc | 125 | else if(southWgt != 0 || eastWgt != 0) |
Hypna | 15:150ec9448efc | 126 | { |
Hypna | 15:150ec9448efc | 127 | if(southWgt > eastWgt) |
Hypna | 15:150ec9448efc | 128 | nextMove = SOUTH; |
Hypna | 15:150ec9448efc | 129 | else |
Hypna | 15:150ec9448efc | 130 | nextMove = EAST; |
Hypna | 15:150ec9448efc | 131 | |
Hypna | 15:150ec9448efc | 132 | path.push(nextMove); |
Hypna | 15:150ec9448efc | 133 | } |
Hypna | 15:150ec9448efc | 134 | else |
Hypna | 15:150ec9448efc | 135 | { |
Hypna | 15:150ec9448efc | 136 | direction lastMove = path.top(); |
Hypna | 15:150ec9448efc | 137 | path.pop(); |
Hypna | 15:150ec9448efc | 138 | |
Hypna | 15:150ec9448efc | 139 | switch(lastMove) |
Hypna | 15:150ec9448efc | 140 | { |
Hypna | 15:150ec9448efc | 141 | case NORTH : nextMove = SOUTH; |
Hypna | 15:150ec9448efc | 142 | break; |
Hypna | 15:150ec9448efc | 143 | case SOUTH : nextMove = NORTH; |
Hypna | 15:150ec9448efc | 144 | break; |
Hypna | 15:150ec9448efc | 145 | case EAST : nextMove = WEST; |
Hypna | 15:150ec9448efc | 146 | break; |
Hypna | 15:150ec9448efc | 147 | case WEST : nextMove = EAST; |
Hypna | 15:150ec9448efc | 148 | } |
Hypna | 15:150ec9448efc | 149 | } |
Hypna | 15:150ec9448efc | 150 | |
Hypna | 15:150ec9448efc | 151 | return nextMove; |
Hypna | 15:150ec9448efc | 152 | } |
Hypna | 15:150ec9448efc | 153 | |
Hypna | 15:150ec9448efc | 154 | void NavController::sendMove(direction next) |
Hypna | 15:150ec9448efc | 155 | { |
Hypna | 15:150ec9448efc | 156 | int diff=next-orient; |
Hypna | 15:150ec9448efc | 157 | |
Hypna | 15:150ec9448efc | 158 | switch(diff) |
Hypna | 15:150ec9448efc | 159 | { |
Hypna | 15:150ec9448efc | 160 | case 0 : |
Hypna | 15:150ec9448efc | 161 | wiringXI2CWrite(frdm_i2c, 'F');//forward |
Hypna | 15:150ec9448efc | 162 | break; |
Hypna | 15:150ec9448efc | 163 | case 1 : |
Hypna | 15:150ec9448efc | 164 | wiringXI2CWrite(frdm_i2c, 'R');//right |
Hypna | 15:150ec9448efc | 165 | break; |
Hypna | 15:150ec9448efc | 166 | case 2 : |
Hypna | 15:150ec9448efc | 167 | wiringXI2CWrite(frdm_i2c, 'B');//back |
Hypna | 15:150ec9448efc | 168 | break; |
Hypna | 15:150ec9448efc | 169 | case 3 : |
Hypna | 15:150ec9448efc | 170 | wiringXI2CWrite(frdm_i2c, 'L');//left |
Hypna | 15:150ec9448efc | 171 | } |
Hypna | 15:150ec9448efc | 172 | } |
Hypna | 15:150ec9448efc | 173 | |
Hypna | 15:150ec9448efc | 174 | void NavController::updateMap() |
Hypna | 15:150ec9448efc | 175 | { |
Hypna | 15:150ec9448efc | 176 | if(!map.getCurVisited()) |
Hypna | 15:150ec9448efc | 177 | { |
Hypna | 15:150ec9448efc | 178 | readSensor(NORTH) ? map.setCurWall(NORTH, CLOSED) : map.setCurWall(NORTH, OPEN); |
Hypna | 15:150ec9448efc | 179 | readSensor(SOUTH) ? map.setCurWall(SOUTH, CLOSED) : map.setCurWall(SOUTH, OPEN); |
Hypna | 15:150ec9448efc | 180 | readSensor(EAST) ? map.setCurWall(EAST, CLOSED) : map.setCurWall(EAST, OPEN); |
Hypna | 15:150ec9448efc | 181 | readSensor(WEST) ? map.setCurWall(WEST, CLOSED) : map.setCurWall(WEST, OPEN); |
Hypna | 15:150ec9448efc | 182 | |
Hypna | 15:150ec9448efc | 183 | vector<char> symbols = readWalls(); |
Hypna | 15:150ec9448efc | 184 | |
Hypna | 15:150ec9448efc | 185 | for(int i = 1; i <= symbols.size(); i++) |
Hypna | 15:150ec9448efc | 186 | map.addCurSymbol(symbols.at(i)); |
Hypna | 15:150ec9448efc | 187 | } |
Hypna | 15:150ec9448efc | 188 | } |
Hypna | 15:150ec9448efc | 189 | |
Hypna | 15:150ec9448efc | 190 | void NavController::saveMap() |
Hypna | 15:150ec9448efc | 191 | { |
Hypna | 15:150ec9448efc | 192 | system("mkdir /media/flash; chmod 770 /media/flash; mount -t vfat /dev/sda /media/flash"); |
Hypna | 15:150ec9448efc | 193 | char fileOut[100], |
Hypna | 15:150ec9448efc | 194 | var[5]; |
Hypna | 15:150ec9448efc | 195 | vector<char> symbols; |
Hypna | 15:150ec9448efc | 196 | position spot; |
Hypna | 15:150ec9448efc | 197 | |
Hypna | 15:150ec9448efc | 198 | for(int i = 0; i < 7; i++) |
Hypna | 15:150ec9448efc | 199 | { |
Hypna | 15:150ec9448efc | 200 | for(int j = 0; j < 7; j++) |
Hypna | 15:150ec9448efc | 201 | { |
Hypna | 15:150ec9448efc | 202 | spot.x = i; |
Hypna | 15:150ec9448efc | 203 | spot.y = j; |
Hypna | 15:150ec9448efc | 204 | symbols = map.getSymbols(spot); |
Hypna | 15:150ec9448efc | 205 | |
Hypna | 15:150ec9448efc | 206 | if(!symbols.empty()) |
Hypna | 15:150ec9448efc | 207 | { |
Hypna | 15:150ec9448efc | 208 | strcat(fileOut, "cd /media/flash; cat > characters.txt; Cell ("); |
Hypna | 15:150ec9448efc | 209 | sprintf(var, "%i", i); |
Hypna | 15:150ec9448efc | 210 | strcat(fileOut, var); |
Hypna | 15:150ec9448efc | 211 | strcat(fileOut, ","); |
Hypna | 15:150ec9448efc | 212 | sprintf(var, "%i", j); |
Hypna | 15:150ec9448efc | 213 | strcat(fileOut, var); |
Hypna | 15:150ec9448efc | 214 | strcat(fileOut, ") : "); |
Hypna | 15:150ec9448efc | 215 | strcat(fileOut, &symbols[0]); |
Hypna | 15:150ec9448efc | 216 | |
Hypna | 15:150ec9448efc | 217 | for(int k = 1; k < symbols.size(); k++) |
Hypna | 15:150ec9448efc | 218 | { |
Hypna | 15:150ec9448efc | 219 | strcat(fileOut, ", "); |
Hypna | 15:150ec9448efc | 220 | strcat(fileOut, &symbols[k]); |
Hypna | 15:150ec9448efc | 221 | } |
Hypna | 15:150ec9448efc | 222 | } |
Hypna | 15:150ec9448efc | 223 | |
Hypna | 15:150ec9448efc | 224 | strcat(fileOut, "/n"); |
Hypna | 15:150ec9448efc | 225 | system(fileOut); |
Hypna | 15:150ec9448efc | 226 | } |
Hypna | 15:150ec9448efc | 227 | } |
Hypna | 15:150ec9448efc | 228 | |
Hypna | 15:150ec9448efc | 229 | system("rmdir /media/flash; umount /dev/sda"); |
Hypna | 15:150ec9448efc | 230 | } |
Hypna | 15:150ec9448efc | 231 | |
Hypna | 15:150ec9448efc | 232 | bool NavController::readSensor( direction wall){ |
Hypna | 15:150ec9448efc | 233 | int diff; |
Hypna | 15:150ec9448efc | 234 | diff=wall-orient; |
Hypna | 15:150ec9448efc | 235 | |
Hypna | 15:150ec9448efc | 236 | switch(diff){ |
Hypna | 15:150ec9448efc | 237 | case 0: |
Hypna | 15:150ec9448efc | 238 | digitalRead(SENS_FORWARD); |
Hypna | 15:150ec9448efc | 239 | case 1: |
Hypna | 15:150ec9448efc | 240 | digitalRead(SENS_RIGHT); |
Hypna | 15:150ec9448efc | 241 | case 2: |
Hypna | 15:150ec9448efc | 242 | digitalRead(SENS_BACK); |
Hypna | 15:150ec9448efc | 243 | case 3: |
Hypna | 15:150ec9448efc | 244 | digitalRead(SENS_LEFT); |
Hypna | 15:150ec9448efc | 245 | } |
Hypna | 15:150ec9448efc | 246 | } |
Hypna | 15:150ec9448efc | 247 | |
Hypna | 15:150ec9448efc | 248 | char NavController::readWall( direction wall) |
Hypna | 15:150ec9448efc | 249 | { |
Hypna | 15:150ec9448efc | 250 | turnCam(wall); |
Hypna | 15:150ec9448efc | 251 | |
Hypna | 15:150ec9448efc | 252 | |
Hypna | 15:150ec9448efc | 253 | cv::VideoCapture cap(0); |
Hypna | 15:150ec9448efc | 254 | if(!cap.isOpened()) //check if we succeeded |
Hypna | 15:150ec9448efc | 255 | { |
Hypna | 15:150ec9448efc | 256 | std::cerr << "ERROR: NavController::readWall( direction) failed to locate or open camera." << endl; |
Hypna | 15:150ec9448efc | 257 | return -1; |
Hypna | 15:150ec9448efc | 258 | } |
Hypna | 15:150ec9448efc | 259 | |
Hypna | 15:150ec9448efc | 260 | cv::Mat snap, gray; |
Hypna | 15:150ec9448efc | 261 | cap >> snap; // get a new frame from camera |
Hypna | 15:150ec9448efc | 262 | cvtColor(snap, gray, CV_BGR2GRAY); |
Hypna | 15:150ec9448efc | 263 | // ...other image pre-processing here... |
Hypna | 15:150ec9448efc | 264 | // Pass it to Tesseract API |
Hypna | 15:150ec9448efc | 265 | tesseract::TessBaseAPI tess; |
Hypna | 15:150ec9448efc | 266 | tess.Init(NULL, "eng", tesseract::OEM_DEFAULT); |
Hypna | 15:150ec9448efc | 267 | tess.SetPageSegMode(tesseract::PSM_SINGLE_BLOCK); |
Hypna | 15:150ec9448efc | 268 | tess.SetImage((uchar*)gray.data, gray.cols, gray.rows, 1, gray.cols); |
Hypna | 15:150ec9448efc | 269 | |
Hypna | 15:150ec9448efc | 270 | // Get the text |
Hypna | 15:150ec9448efc | 271 | char* outPtr = tess.GetUTF8Text(); |
Hypna | 15:150ec9448efc | 272 | char out = *outPtr; |
Hypna | 15:150ec9448efc | 273 | |
Hypna | 15:150ec9448efc | 274 | delete [] outPtr; |
Hypna | 15:150ec9448efc | 275 | |
Hypna | 15:150ec9448efc | 276 | return out; |
Hypna | 15:150ec9448efc | 277 | } |
Hypna | 15:150ec9448efc | 278 | |
Hypna | 15:150ec9448efc | 279 | vector<char> NavController::readWalls() |
Hypna | 15:150ec9448efc | 280 | { |
Hypna | 15:150ec9448efc | 281 | vector<char> out; |
Hypna | 15:150ec9448efc | 282 | |
Hypna | 15:150ec9448efc | 283 | if(map.getCurWall(NORTH) == CLOSED) |
Hypna | 15:150ec9448efc | 284 | out.push_back(readWall(NORTH)); |
Hypna | 15:150ec9448efc | 285 | if(map.getCurWall(SOUTH) == CLOSED) |
Hypna | 15:150ec9448efc | 286 | out.push_back(readWall(SOUTH)); |
Hypna | 15:150ec9448efc | 287 | if(map.getCurWall(EAST) == CLOSED) |
Hypna | 15:150ec9448efc | 288 | out.push_back(readWall(EAST)); |
Hypna | 15:150ec9448efc | 289 | if(map.getCurWall(WEST) == CLOSED) |
Hypna | 15:150ec9448efc | 290 | out.push_back(readWall(WEST)); |
Hypna | 15:150ec9448efc | 291 | |
Hypna | 15:150ec9448efc | 292 | resetCam(); |
Hypna | 15:150ec9448efc | 293 | |
Hypna | 15:150ec9448efc | 294 | return out; |
Hypna | 15:150ec9448efc | 295 | } |
Hypna | 15:150ec9448efc | 296 | |
Hypna | 15:150ec9448efc | 297 | void NavController::initIO(){ |
Hypna | 15:150ec9448efc | 298 | if (wiringXSetup() < 0) |
Hypna | 15:150ec9448efc | 299 | { |
Hypna | 15:150ec9448efc | 300 | cout << "failed to initialize wiringX"; |
Hypna | 15:150ec9448efc | 301 | exit (1) ; |
Hypna | 15:150ec9448efc | 302 | } |
Hypna | 15:150ec9448efc | 303 | //pin initializations go here |
Hypna | 15:150ec9448efc | 304 | pinMode(6, OUTPUT); |
Hypna | 15:150ec9448efc | 305 | pinMode(SENS_FORWARD, INPUT); |
Hypna | 15:150ec9448efc | 306 | pinMode(SENS_RIGHT, INPUT); |
Hypna | 15:150ec9448efc | 307 | pinMode(SENS_BACK, INPUT); |
Hypna | 15:150ec9448efc | 308 | pinMode(SENS_LEFT, INPUT); |
Hypna | 15:150ec9448efc | 309 | pinMode(STEP, OUTPUT); |
Hypna | 15:150ec9448efc | 310 | pinMode(STEP_DIR, OUTPUT); |
Hypna | 15:150ec9448efc | 311 | if((frdm_i2c=wiringXI2CSetup(FRDM_ADD))<0){ |
Hypna | 15:150ec9448efc | 312 | cerr << "ERROR: Failed to establish I2C connection"<<endl; |
Hypna | 15:150ec9448efc | 313 | } |
Hypna | 15:150ec9448efc | 314 | } |
Hypna | 15:150ec9448efc | 315 | |
Hypna | 15:150ec9448efc | 316 | void NavController::turnCam( typename ::direction wall){ |
Hypna | 15:150ec9448efc | 317 | int diff; |
Hypna | 15:150ec9448efc | 318 | diff=wall-camorient; |
Hypna | 15:150ec9448efc | 319 | |
Hypna | 15:150ec9448efc | 320 | if(abs(diff)==3){ |
Hypna | 15:150ec9448efc | 321 | diff/=-3; |
Hypna | 15:150ec9448efc | 322 | } |
Hypna | 15:150ec9448efc | 323 | |
Hypna | 15:150ec9448efc | 324 | if(diff<0){ |
Hypna | 15:150ec9448efc | 325 | digitalWrite(STEP_DIR, HIGH); |
Hypna | 15:150ec9448efc | 326 | diff=abs(diff); |
Hypna | 15:150ec9448efc | 327 | } |
Hypna | 15:150ec9448efc | 328 | else{ |
Hypna | 15:150ec9448efc | 329 | digitalWrite(STEP_DIR, LOW); |
Hypna | 15:150ec9448efc | 330 | } |
Hypna | 15:150ec9448efc | 331 | |
Hypna | 15:150ec9448efc | 332 | diff*=1024; |
Hypna | 15:150ec9448efc | 333 | |
Hypna | 15:150ec9448efc | 334 | for(int i=0; i<diff; i++){ |
Hypna | 15:150ec9448efc | 335 | digitalWrite(STEP, HIGH); |
Hypna | 15:150ec9448efc | 336 | delayMicroseconds(5000); //.5 millisecond |
Hypna | 15:150ec9448efc | 337 | digitalWrite(STEP, HIGH); |
Hypna | 15:150ec9448efc | 338 | delayMicroseconds(5000); |
Hypna | 15:150ec9448efc | 339 | } |
Hypna | 15:150ec9448efc | 340 | } |
Hypna | 15:150ec9448efc | 341 | |
Hypna | 15:150ec9448efc | 342 | void NavController::resetCam(){ |
Hypna | 15:150ec9448efc | 343 | int diff; |
Hypna | 15:150ec9448efc | 344 | diff=orient-camorient; |
Hypna | 15:150ec9448efc | 345 | |
Hypna | 15:150ec9448efc | 346 | if(abs(diff)==3){ |
Hypna | 15:150ec9448efc | 347 | diff/=-3; |
Hypna | 15:150ec9448efc | 348 | } |
Hypna | 15:150ec9448efc | 349 | |
Hypna | 15:150ec9448efc | 350 | if(diff<0){ |
Hypna | 15:150ec9448efc | 351 | digitalWrite(STEP_DIR, HIGH); |
Hypna | 15:150ec9448efc | 352 | diff=abs(diff); |
Hypna | 15:150ec9448efc | 353 | } |
Hypna | 15:150ec9448efc | 354 | else{ |
Hypna | 15:150ec9448efc | 355 | digitalWrite(STEP_DIR, LOW); |
Hypna | 15:150ec9448efc | 356 | } |
Hypna | 15:150ec9448efc | 357 | |
Hypna | 15:150ec9448efc | 358 | diff*=1024; |
Hypna | 15:150ec9448efc | 359 | |
Hypna | 15:150ec9448efc | 360 | for(int i=0; i<diff; i++){ |
Hypna | 15:150ec9448efc | 361 | digitalWrite(STEP, HIGH); |
Hypna | 15:150ec9448efc | 362 | delayMicroseconds(5000); //millisecond |
Hypna | 15:150ec9448efc | 363 | digitalWrite(STEP, HIGH); |
Hypna | 15:150ec9448efc | 364 | delayMicroseconds(5000); |
Hypna | 15:150ec9448efc | 365 | } |
Hypna | 15:150ec9448efc | 366 | } |
Hypna | 15:150ec9448efc | 367 | |
Hypna | 15:150ec9448efc | 368 | void NavController::pinWrite(int pin, int lvl){ |
Hypna | 15:150ec9448efc | 369 | if(lvl == 1) |
Hypna | 15:150ec9448efc | 370 | digitalWrite(pin, HIGH); |
Hypna | 15:150ec9448efc | 371 | else |
Hypna | 15:150ec9448efc | 372 | digitalWrite(pin, LOW); |
Hypna | 15:150ec9448efc | 373 | } |
Hypna | 15:150ec9448efc | 374 | |
Hypna | 15:150ec9448efc | 375 | void NavController::waitingForInterrupt(int pin, int set){ |
Hypna | 15:150ec9448efc | 376 | waitForInterrupt(pin, set); |
Hypna | 15:150ec9448efc | 377 | } |
Hypna | 15:150ec9448efc | 378 | |
Hypna | 15:150ec9448efc | 379 | void NavController::testWithDelay() |
Hypna | 15:150ec9448efc | 380 | { |
Hypna | 15:150ec9448efc | 381 | while(true) |
Hypna | 15:150ec9448efc | 382 | { |
Hypna | 15:150ec9448efc | 383 | sendMove(NORTH); |
Hypna | 15:150ec9448efc | 384 | usleep(500); |
Hypna | 15:150ec9448efc | 385 | sendMove(WEST); |
Hypna | 15:150ec9448efc | 386 | usleep(500); |
Hypna | 15:150ec9448efc | 387 | sendMove(SOUTH); |
Hypna | 15:150ec9448efc | 388 | usleep(500); |
Hypna | 15:150ec9448efc | 389 | sendMove(EAST); |
Hypna | 15:150ec9448efc | 390 | usleep(500); |
Hypna | 15:150ec9448efc | 391 | } |
Hypna | 15:150ec9448efc | 392 | } |
Hypna | 15:150ec9448efc | 393 | |
Hypna | 15:150ec9448efc | 394 | void NavController::testWithoutDelay() |
Hypna | 15:150ec9448efc | 395 | { |
Hypna | 15:150ec9448efc | 396 | while(true) |
Hypna | 15:150ec9448efc | 397 | { |
Hypna | 15:150ec9448efc | 398 | sendMove(NORTH); |
Hypna | 15:150ec9448efc | 399 | sendMove(WEST); |
Hypna | 15:150ec9448efc | 400 | sendMove(SOUTH); |
Hypna | 15:150ec9448efc | 401 | sendMove(EAST); |
Hypna | 15:150ec9448efc | 402 | } |
Hypna | 15:150ec9448efc | 403 | } |