API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Jul 20th, 2017
52
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 11.12 KB | None | 0 0
raw download clone embed print report
  1. /* drive.c
  2.  * Designed to run on Create Command Module
  3.  *
  4.  * The basic architecture of this program can be re-used to easily
  5.  * write a wide variety of Create control programs.  All sensor values
  6.  * are polled in the background (using the serial rx interrupt) and
  7.  * stored in the sensors array as long as the function
  8.  * delayAndUpdateSensors() is called periodically.  Users can send commands
  9.  * directly a byte at a time using byteTx() or they can use the
  10.  * provided functions, such as baud() and drive().
  11.  */
  12.  
  13.  
  14.  
  15. // Includes
  16. #include <avr/interrupt.h>
  17. #include <avr/io.h>
  18. #include <stdlib.h>
  19. #include "oi.h"
  20.  
  21.  
  22. // Constants
  23. #define RESET_SONG 0
  24. #define START_SONG 1
  25. #define BUMP_SONG  2
  26. #define END_SONG   3
  27.  
  28.  
  29. // Global variables
  30. volatile uint16_t timer_cnt = 0;
  31. volatile uint8_t timer_on = 0;
  32. volatile uint8_t sensors_flag = 0;
  33. volatile uint8_t sensors_index = 0;
  34. volatile uint8_t sensors_in[Sen6Size];
  35. volatile uint8_t sensors[Sen6Size];
  36. volatile int16_t distance = 0;
  37. volatile int16_t angle = 0;
  38.  
  39.  
  40.  
  41.  
  42. // Functions
  43. void byteTx(uint8_t value);
  44. void delayMs(uint16_t time_ms);
  45. void delayAndUpdateSensors(unsigned int time_ms);
  46. void initialize(void);
  47. void powerOnRobot(void);
  48. void baud(uint8_t baud_code);
  49. void drive(int16_t velocity, int16_t radius);
  50. uint16_t randomAngle(void);
  51. void defineSongs(void);
  52.  
  53. int main (void)
  54. {
  55.   uint8_t leds_cnt = 99;
  56.   uint8_t leds_state = 0;
  57.   uint8_t leds_on = 1;
  58.  
  59.   int16_t turn_angle = 0;
  60.   uint8_t turn_dir = 1;
  61.   uint8_t turning = 0;
  62.   uint8_t backing_up = 0;
  63.  
  64.   uint8_t turns = 0;
  65.   uint8_t finished = 0;
  66.  
  67.  
  68.   // Set up Create and module
  69.   initialize();
  70.   LEDBothOff;
  71.   powerOnRobot();
  72.   byteTx(CmdStart);
  73.   baud(Baud28800);
  74.   defineSongs();
  75.   byteTx(CmdControl);
  76.   byteTx(CmdFull);
  77.  
  78.   // Stop just as a precaution
  79.   drive(0, RadStraight);
  80.  
  81.   // Play the reset song and wait while it plays
  82.   byteTx(CmdPlay);
  83.   byteTx(RESET_SONG);
  84.   delayAndUpdateSensors(750);
  85.  
  86.  
  87.   for(;;)
  88.   {
  89.  
  90.     if(++leds_cnt >= 100)
  91.     {
  92.       leds_cnt = 0;
  93.       leds_on = !leds_on;
  94.  
  95.       if(leds_on)
  96.       {
  97.         byteTx(CmdLeds);
  98.         byteTx(LEDsBoth);
  99.         byteTx(128);
  100.         byteTx(255);
  101.         LEDBothOff;
  102.       }
  103.       else
  104.       {
  105.         byteTx(CmdLeds);
  106.         byteTx(0x00);
  107.         byteTx(0);
  108.         byteTx(0);
  109.         LEDBothOn;
  110.       }
  111.     }
  112.  
  113.     delayAndUpdateSensors(10);
  114.  
  115.     if(UserButtonPressed)
  116.     {
  117.         finished = 0;
  118.         turns = 0;
  119.    
  120.       // Play start song and wait
  121.       byteTx(CmdPlay);
  122.       byteTx(START_SONG);
  123.       delayAndUpdateSensors(2813);
  124.      
  125.  
  126.       // Drive around until a button or unsafe condition is detected
  127.       while(!(UserButtonPressed)
  128.             && (!sensors[SenCliffL])
  129.             && (!sensors[SenCliffFL])
  130.             && (!sensors[SenCliffFR])
  131.             && (!sensors[SenCliffR])
  132.             && (!sensors[SenChAvailable]
  133.             && (finished == 0))
  134.         )
  135.       {
  136.  
  137.         // Keep turning until the specified angle is reached
  138.         if(turning)
  139.         {
  140.           if(backing_up)
  141.           {
  142.             if((-distance) > 5)
  143.               backing_up = 0;
  144.             drive(-200, RadStraight);
  145.           }
  146.           else
  147.           {
  148.             if (turns == 3)
  149.             {
  150.                 finished = 1;
  151.             }
  152.            
  153.             if(turn_dir)
  154.             {
  155.               if(angle > turn_angle)
  156.               {
  157.                 turning = 0;
  158.                 distance = 0;
  159.                 turns++;
  160.               }
  161.               drive(200, RadCCW);
  162.             }
  163.             else
  164.             {
  165.               if((-angle) > turn_angle)
  166.               {
  167.                 turning = 0;
  168.                 distance = 0;
  169.                 turns++;
  170.               }
  171.               drive(200, RadCW);
  172.             }
  173.           }
  174.         }
  175.         else if(sensors[SenBumpDrop] & BumpEither)  // Check for a bump
  176.         {
  177.           // Set the turn parameters and reset the angle
  178.           if(sensors[SenBumpDrop] & BumpLeft)
  179.             turn_dir = 0;
  180.           else
  181.             turn_dir = 1;
  182.           backing_up = 1;
  183.           turning = 1;
  184.           distance = 0;
  185.           angle = 0;
  186.           turn_angle = randomAngle();
  187.  
  188.           // Play the bump song
  189.           byteTx(CmdPlay);
  190.           byteTx(BUMP_SONG);
  191.         }
  192.         else
  193.         {
  194.             // Otherwise, drive!
  195.          
  196.             if (distance > 1080)
  197.             {
  198.                 turn_dir = 0;
  199.                 turning = 1;
  200.                 angle = 0;
  201.                 turn_angle = 80;
  202.             }
  203.             else
  204.             {
  205.                 if (turns == 4)
  206.                 {
  207.                     finished = 1;
  208.                 }
  209.                 else
  210.                 {
  211.                     drive(300, RadStraight);
  212.                 }
  213.             }
  214.         }
  215.  
  216.  
  217.         // Flash the leds in sequence
  218.         if(++leds_cnt >= 10)
  219.         {
  220.           leds_cnt = 0;
  221.           if(turning)
  222.           {
  223.             // Flash backward while turning
  224.             if(leds_state == 0)
  225.               leds_state = 4;
  226.             else
  227.               leds_state--;
  228.           }
  229.           else
  230.           {
  231.             if(leds_state == 4)
  232.               leds_state = 0;
  233.             else
  234.               leds_state++;
  235.           }
  236.  
  237.           if(leds_state == 0)
  238.           {
  239.             // robot Power LED Amber
  240.             byteTx(CmdLeds);
  241.             byteTx(0x00);
  242.             byteTx(128);
  243.             byteTx(255);
  244.             LEDBothOff;
  245.           }
  246.           else if(leds_state == 1)
  247.           {
  248.             // Play LED on
  249.             byteTx(CmdLeds);
  250.             byteTx(LEDPlay);
  251.             byteTx(0);
  252.             byteTx(0);
  253.             LEDBothOff;
  254.           }
  255.           else if(leds_state == 2)
  256.           {
  257.             // Advance LED on
  258.             byteTx(CmdLeds);
  259.             byteTx(LEDAdvance);
  260.             byteTx(0);
  261.             byteTx(0);
  262.             LEDBothOff;
  263.           }
  264.           else if(leds_state == 3)
  265.           {
  266.             // Robot LEDs off, CM left LED on
  267.             byteTx(CmdLeds);
  268.             byteTx(0x00);
  269.             byteTx(0);
  270.             byteTx(0);
  271.             LED2On;
  272.             LED1Off;
  273.           }
  274.           else if(leds_state == 4)
  275.           {
  276.             // Robot LEDs off, CM right LED on
  277.             byteTx(CmdLeds);
  278.             byteTx(0x00);
  279.             byteTx(0);
  280.             byteTx(0);
  281.             LED1On;
  282.             LED2Off;
  283.           }
  284.         }
  285.  
  286.         // wait a little more than one robot tick for sensors to update
  287.         delayAndUpdateSensors(20);
  288.       }
  289.  
  290.       // Stop driving
  291.       drive(0, RadStraight);
  292.  
  293.       // Play end song and wait
  294.       delayAndUpdateSensors(500);
  295.       byteTx(CmdPlay);
  296.       byteTx(END_SONG);
  297.       delayAndUpdateSensors(2438);
  298.     }
  299.   }
  300. }
  301.  
  302.  
  303.  
  304.  
  305. // Serial receive interrupt to store sensor values
  306. SIGNAL(SIG_USART_RECV)
  307. {
  308.   uint8_t temp;
  309.  
  310.  
  311.   temp = UDR0;
  312.  
  313.   if(sensors_flag)
  314.   {
  315.     sensors_in[sensors_index++] = temp;
  316.     if(sensors_index >= Sen6Size)
  317.       sensors_flag = 0;
  318.   }
  319. }
  320.  
  321.  
  322.  
  323.  
  324. // Timer 1 interrupt to time delays in ms
  325. SIGNAL(SIG_OUTPUT_COMPARE1A)
  326. {
  327.   if(timer_cnt)
  328.     timer_cnt--;
  329.   else
  330.     timer_on = 0;
  331. }
  332.  
  333.  
  334.  
  335.  
  336. // Transmit a byte over the serial port
  337. void byteTx(uint8_t value)
  338. {
  339.   while(!(UCSR0A & _BV(UDRE0))) ;
  340.   UDR0 = value;
  341. }
  342.  
  343.  
  344.  
  345.  
  346. // Delay for the specified time in ms without updating sensor values
  347. void delayMs(uint16_t time_ms)
  348. {
  349.   timer_on = 1;
  350.   timer_cnt = time_ms;
  351.   while(timer_on) ;
  352. }
  353.  
  354.  
  355.  
  356.  
  357. // Delay for the specified time in ms and update sensor values
  358. void delayAndUpdateSensors(uint16_t time_ms)
  359. {
  360.   uint8_t temp;
  361.  
  362.   timer_on = 1;
  363.   timer_cnt = time_ms;
  364.   while(timer_on)
  365.   {
  366.     if(!sensors_flag)
  367.     {
  368.       for(temp = 0; temp < Sen6Size; temp++)
  369.         sensors[temp] = sensors_in[temp];
  370.  
  371.       // Update running totals of distance and angle
  372.       distance += (int)((sensors[SenDist1] << 8) | sensors[SenDist0]);
  373.       angle += (int)((sensors[SenAng1] << 8) | sensors[SenAng0]);
  374.  
  375.       byteTx(CmdSensors);
  376.       byteTx(6);
  377.       sensors_index = 0;
  378.       sensors_flag = 1;
  379.     }
  380.   }
  381. }
  382.  
  383.  
  384.  
  385.  
  386. // Initialize the Mind Control's ATmega168 microcontroller
  387. void initialize(void)
  388. {
  389.   cli();
  390.  
  391.   // Set I/O pins
  392.   DDRB = 0x10;
  393.   PORTB = 0xCF;
  394.   DDRC = 0x00;
  395.   PORTC = 0xFF;
  396.   DDRD = 0xE6;
  397.   PORTD = 0x7D;
  398.  
  399.   // Set up timer 1 to generate an interrupt every 1 ms
  400.   TCCR1A = 0x00;
  401.   TCCR1B = (_BV(WGM12) | _BV(CS12));
  402.   OCR1A = 71;
  403.   TIMSK1 = _BV(OCIE1A);
  404.  
  405.   // Set up the serial port with rx interrupt
  406.   UBRR0 = 19;
  407.   UCSR0B = (_BV(RXCIE0) | _BV(TXEN0) | _BV(RXEN0));
  408.   UCSR0C = (_BV(UCSZ00) | _BV(UCSZ01));
  409.  
  410.   // Turn on interrupts
  411.   sei();
  412. }
  413.  
  414.  
  415.  
  416.  
  417. void powerOnRobot(void)
  418. {
  419.   // If Create's power is off, turn it on
  420.   if(!RobotIsOn)
  421.   {
  422.       while(!RobotIsOn)
  423.       {
  424.           RobotPwrToggleLow;
  425.           delayMs(500);  // Delay in this state
  426.           RobotPwrToggleHigh;  // Low to high transition to toggle power
  427.           delayMs(100);  // Delay in this state
  428.           RobotPwrToggleLow;
  429.       }
  430.       delayMs(3500);  // Delay for startup
  431.   }
  432. }
  433.  
  434.  
  435.  
  436.  
  437. // Switch the baud rate on both Create and module
  438. void baud(uint8_t baud_code)
  439. {
  440.   if(baud_code <= 11)
  441.   {
  442.     byteTx(CmdBaud);
  443.     UCSR0A |= _BV(TXC0);
  444.     byteTx(baud_code);
  445.     // Wait until transmit is complete
  446.     while(!(UCSR0A & _BV(TXC0))) ;
  447.  
  448.     cli();
  449.  
  450.     // Switch the baud rate register
  451.     if(baud_code == Baud115200)
  452.       UBRR0 = Ubrr115200;
  453.     else if(baud_code == Baud57600)
  454.       UBRR0 = Ubrr57600;
  455.     else if(baud_code == Baud38400)
  456.       UBRR0 = Ubrr38400;
  457.     else if(baud_code == Baud28800)
  458.       UBRR0 = Ubrr28800;
  459.     else if(baud_code == Baud19200)
  460.       UBRR0 = Ubrr19200;
  461.     else if(baud_code == Baud14400)
  462.       UBRR0 = Ubrr14400;
  463.     else if(baud_code == Baud9600)
  464.       UBRR0 = Ubrr9600;
  465.     else if(baud_code == Baud4800)
  466.       UBRR0 = Ubrr4800;
  467.     else if(baud_code == Baud2400)
  468.       UBRR0 = Ubrr2400;
  469.     else if(baud_code == Baud1200)
  470.       UBRR0 = Ubrr1200;
  471.     else if(baud_code == Baud600)
  472.       UBRR0 = Ubrr600;
  473.     else if(baud_code == Baud300)
  474.       UBRR0 = Ubrr300;
  475.  
  476.     sei();
  477.  
  478.     delayMs(100);
  479.   }
  480. }
  481.  
  482.  
  483.  
  484.  
  485. // Send Create drive commands in terms of velocity and radius
  486. void drive(int16_t velocity, int16_t radius)
  487. {
  488.   byteTx(CmdDrive);
  489.   byteTx((uint8_t)((velocity >> 8) & 0x00FF));
  490.   byteTx((uint8_t)(velocity & 0x00FF));
  491.   byteTx((uint8_t)((radius >> 8) & 0x00FF));
  492.   byteTx((uint8_t)(radius & 0x00FF));
  493. }
  494.  
  495.  
  496.  
  497.  
  498. // Return an angle value in the range 53 to 180 (degrees)
  499. uint16_t randomAngle(void)
  500. {
  501.     return (53 + ((uint16_t)(random() & 0xFF) >> 1));
  502. }
  503.  
  504.  
  505.  
  506. // Define songs to be played later
  507. void defineSongs(void)
  508. {
  509.   // Reset song
  510.   byteTx(CmdSong);
  511.   byteTx(RESET_SONG);
  512.   byteTx(4);
  513.   byteTx(60);
  514.   byteTx(6);
  515.   byteTx(72);
  516.   byteTx(6);
  517.   byteTx(84);
  518.   byteTx(6);
  519.   byteTx(96);
  520.   byteTx(6);
  521.  
  522.   // Start song
  523.   byteTx(CmdSong);
  524.   byteTx(START_SONG);
  525.   byteTx(6);
  526.   byteTx(69);
  527.   byteTx(18);
  528.   byteTx(72);
  529.   byteTx(12);
  530.   byteTx(74);
  531.   byteTx(12);
  532.   byteTx(72);
  533.   byteTx(12);
  534.   byteTx(69);
  535.   byteTx(12);
  536.   byteTx(77);
  537.   byteTx(24);
  538.  
  539.   // Bump song
  540.   byteTx(CmdSong);
  541.   byteTx(BUMP_SONG);
  542.   byteTx(2);
  543.   byteTx(74);
  544.   byteTx(12);
  545.   byteTx(59);
  546.   byteTx(24);
  547.  
  548.   // End song
  549.   byteTx(CmdSong);
  550.   byteTx(END_SONG);
  551.   byteTx(5);
  552.   byteTx(77);
  553.   byteTx(18);
  554.   byteTx(74);
  555.   byteTx(12);
  556.   byteTx(72);
  557.   byteTx(12);
  558.   byteTx(69);
  559.   byteTx(12);
  560.   byteTx(65);
  561.   byteTx(24);
  562. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!