Pastebin launched a little side project called HostCabi.net, check it out ;-)Don't like ads? PRO users don't see any ads ;-)
Guest

Myro

By: a guest on Nov 14th, 2012  |  syntax: C++  |  size: 5.81 KB  |  hits: 14  |  expires: Never
download  |  raw  |  embed  |  report abuse  |  print
Text below is selected. Please press Ctrl+C to copy to your clipboard. (⌘+C on Mac)
  1. //Jesse Chand
  2. //Team WU TANG: Tam Henry Le Nguyen, Denzel Alexander, Adrian Mendoza
  3. //Dr. Sheila Tejada
  4. //11 November 2012
  5.  
  6. //Urban Search and Rescue
  7. //-----------------------
  8.  
  9. #include <iostream>
  10. #include <cstdlib>
  11. #include "Myro.h"
  12. #include "math.h"
  13. #include "stdio.h"
  14. #include <string.h>
  15.  
  16. using namespace std;
  17.  
  18. //Inch forward by a bit
  19. void crawlForward()
  20. {
  21.     robot.motors(1,1);
  22.     wait(1);
  23.     robot.motors(0,0);
  24. }
  25.  
  26. //Inch backward by a bit
  27. void crawlBackward()
  28. {
  29.     robot.motors(-1,-1);
  30.     wait(1);
  31.     robot.motors(0,0);
  32. }
  33.  
  34. //Tell the user all the instructions.
  35. void Intro()
  36. {
  37.     cout << "Remote controlling robot. Press WASD to navigate, F to brake." << endl;
  38.     cout << "Press P to take a picture." << endl;
  39.     cout << "Press Q to alert the user with a beep." << endl;
  40.     cout << "When you are done searching, press C to complete and view the slideshow." << endl;    
  41.    
  42.     cout << "Robot statistics:" << endl;
  43.     cout << "Robot Obstacle Sensor Check: " << robot.getObstacle("center") << endl;
  44.     cout << "Robot Light Sensor Check: " << robot.getLight("center") << endl;
  45.     cout << "Robot IR Check: " << robot.getIR("left") << endl;
  46. }
  47.  
  48. //Movement Control
  49. void remoteControl()
  50. {
  51.     //This character stores the last command the user inputted.
  52.     //WASD to move, Q to brake.
  53.    
  54.     char index = 'n';
  55.     char response = 'z';
  56.    
  57.     double timer = currentTime();
  58.    
  59.     PicturePtr pLibrary[100];  //Original pictures
  60.     PicturePtr Found[100];     //Modified pictures
  61.     int imgindex = 0;          //Number of pictures taken
  62.     int foundIndex = 0;        //Number of found robots
  63.    
  64.     const int WIDTH = 256;
  65.     const int HEIGHT = 192;
  66.    
  67.     int x1, x2, y1, y2;        //Four points define the user-inputted rectangle
  68.    
  69.     Intro();
  70.    
  71.     while(true) {
  72.        
  73.     cin >> index;
  74.    
  75.     //Move Forward
  76.     if (index == 'w') {
  77.         crawlForward();
  78.         index = 'n';
  79.     }
  80.     //Alert Driver: Location
  81.     if (index == 'q') {
  82.         robot.beep(0.25,4000);
  83.         robot.beep(0.25,4000,5000);
  84.         robot.beep(0.25,4000);
  85.         robot.beep(0.25,4000,5000);
  86.         robot.beep(0.25,4000);
  87.         robot.beep(0.25,4000,5000);
  88.         robot.beep(0.25,4000);
  89.         robot.beep(0.25,4000,5000);
  90.     }
  91.     //Turn Left 90 degrees
  92.     else if (index == 'a') {
  93.         robot.motors(-1,1);
  94.         wait(0.78);
  95.         robot.motors(0,0);
  96.         index = 'n';
  97.     }
  98.     //Move Backward
  99.     else if (index == 's') {
  100.         crawlBackward();
  101.         index = 'n';
  102.     }
  103.     //Turn Right 90 degrees
  104.     else if (index == 'd') {
  105.         robot.motors(1,-1);
  106.         wait(0.78);
  107.         robot.motors(0,0);
  108.         index = 'n';
  109.     }
  110.     //Brake
  111.     else if (index == 'f') {
  112.         robot.motors(0,0);
  113.         wait(1);
  114.         robot.motors(0,0);
  115.         index = 'n';
  116.     }
  117.     //Take Picture
  118.     else if (index == 'p') {
  119.         cout << "Taking Picture ..." << endl;
  120.         robot.motors(0,0);
  121.         timer = currentTime();
  122.        
  123.         pLibrary[imgindex] = robot.takePicture();
  124.        
  125.         //For convenience, draw a grid over the picture.
  126.         //Create a blue grid over the image.
  127.         for (int xx = 0; xx < WIDTH; xx += WIDTH/4) {
  128.             for (int yy = 0; yy < HEIGHT; yy++) {
  129.                 setPixelColor(pLibrary[imgindex],xx,yy,0,0,255);
  130.             }
  131.         }
  132.         for (int yy = 0; yy < HEIGHT; yy += HEIGHT/4) {
  133.             for (int xx = 0; xx < WIDTH; xx++) {
  134.                 setPixelColor(pLibrary[imgindex],xx,yy,0,0,255);
  135.             }
  136.         }
  137.        
  138.         cout << "Displaying picture # " << (imgindex+1) << ". Rendering time: " << (currentTime() - timer) << "s." << endl;
  139.        
  140.         show(pLibrary[imgindex]);
  141.         cout << "Is there a robot in this image? Press 'y' or 'n'." << endl;
  142.         cin >> response;
  143.        
  144.         if (response == 'y') {
  145.             cout << "Locate the robot. Enter x1, y1, x2, y2." << endl;
  146.             Found[foundIndex] = pLibrary[imgindex]->clone();
  147.            
  148.             cin >> x1 >> y1 >> x2 >> y2;
  149.            
  150.             //Draw the rectangle in red.
  151.             for (int xx = x1; xx < x2; xx++) {
  152.                 setPixelColor(Found[foundIndex],xx,y1,255,0,0);    //Upper edge
  153.                 setPixelColor(Found[foundIndex],xx,y2,255,0,0);    //Lower edge
  154.             }
  155.             for (int yy = y1; yy < y2; yy++) {
  156.                 setPixelColor(Found[foundIndex],x1,yy,255,0,0);    //Upper edge
  157.                 setPixelColor(Found[foundIndex],x2,yy,255,0,0);    //Lower edge
  158.             }
  159.            
  160.             cout << "Displaying located robot ..." << endl;
  161.             show(Found[foundIndex]);
  162.             foundIndex++;
  163.         }
  164.         response = 'z';
  165.        
  166.         imgindex++;
  167.         index = 'n';
  168.         cout << "Picture has been edited. Returning to movement control." << endl;
  169.     }
  170.     //Slideshow
  171.     else if (index == 'c') {
  172.         cout << "Showing slideshow of images taken." << endl;
  173.        
  174.         for (int i = 0; i < imgindex; i++) {
  175.             char name[9] = "picx.jpg";
  176.             name[3] = i;
  177.             savePicture(pLibrary[i], name);
  178.             show(pLibrary[0]);
  179.         }
  180.        
  181.         cout << "Showing slideshow of modified images." << endl;
  182.        
  183.         for (int i = 0; i < foundIndex; i++) {
  184.             char name[11] = "foundx.jpg";
  185.             name[5] = i;
  186.             savePicture(Found[i], name);
  187.             show(Found[i]);
  188.         }
  189.        
  190.         cout << "Slideshow complete. Returning to movement control." << endl;
  191.        
  192.         index = 'n';
  193.     }
  194.        
  195.     }
  196. }
  197.  
  198. int main()
  199. {
  200.         connect("/dev/tty.StarvingArtist");
  201.     cout << "Battery Level: " << (robot.getBattery() / 9 * 100) << "%." << endl;
  202.    
  203.     remoteControl();
  204.    
  205.     disconnect();
  206.     return 0;
  207. }