API tools faq
paste
Advertisement
harinin

Untitled

harinin
Aug 3rd, 2016
262
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C (WinAPI) 5.28 KB | None | 0 0
raw download clone embed print report
  1. /*
  2.  * File:   runStateMachine.c
  3.  * Author: max
  4.  *
  5.  * Created on July 28, 2016, 12:43 PM
  6.  */
  7.  
  8. #include "roach.h"
  9. #include "runStateMachine.h"
  10. #include "events.h"
  11. #include "CalibConstants.h"
  12. #include "Navigation.h"
  13. #include <stdio.h>
  14. #include <timers.h>
  15.  
  16. #define MAX_SPEED 10
  17. #define TIME_BACK_AWAY 3000
  18. #define TIME_ARC_RIGHT 3000
  19.  
  20. void RunStateMachine(event_t currentEvent) {
  21.     static state_t currentState = INIT;
  22.     //which state are we in?
  23.     printf("Iteration - State: %d\tEvent: %d\n", currentState, currentEvent);
  24.  
  25.     switch (currentState) {
  26.         case INIT:
  27.             Forward(MAX_SPEED);
  28.             currentState = FORWARD;
  29.             break;
  30.         case FORWARD:
  31.             switch (currentEvent) {
  32.                 case FRONT_BUMP_DOWN:
  33.                     Reverse(MAX_SPEED);
  34.                     currentState = BACK;
  35.                     break;
  36.                 case FL_BUMP_DOWN:
  37.                     RotateRight(MAX_SPEED);
  38.                     InitTimer(0, TIME_TURN_360 / 4);
  39.                     currentState = TURNRIGHT;
  40.                     break;
  41.                 case FR_BUMP_DOWN:
  42.                     RotateLeft(MAX_SPEED);
  43.                     InitTimer(0, TIME_TURN_360 / 4);
  44.                     currentState = TURNLEFT;
  45.                     break;
  46.                 case ENTERED_DARK:
  47.                     Stop();
  48.                     currentState = DARKIDLE;
  49.                     break;
  50.             }
  51.             break;
  52.         case TURNLEFT:
  53.             switch (currentEvent) {
  54.                 case TIMER_0_EXPIRED:
  55.                     ClearTimerExpired(0);
  56.                     Forward(MAX_SPEED);
  57.                     currentState = FORWARD;
  58.                     break;
  59.                 case FRONT_BUMP_DOWN:
  60.                     InitTimer(0, TIME_BACK_AWAY);
  61.                     Reverse(MAX_SPEED);
  62.                     currentState = BACK;
  63.                     break;
  64.                 case FL_BUMP_DOWN:
  65.                     RotateRight(MAX_SPEED);
  66.                     InitTimer(0, TIME_TURN_360 / 4);
  67.                     currentState = TURNRIGHT;
  68.                     break;
  69.                 case ENTERED_DARK:
  70.                     StopTimer(0);
  71.                     ClearTimerExpired(0);
  72.                     Stop();
  73.                     currentState = DARKIDLE;
  74.                     break;
  75.             }
  76.             break;
  77.         case TURNRIGHT:
  78.             switch (currentEvent) {
  79.                 case TIMER_0_EXPIRED:
  80.                     ClearTimerExpired(0);
  81.                     Forward(MAX_SPEED);
  82.                     currentState = FORWARD;
  83.                     break;
  84.                 case FRONT_BUMP_DOWN:
  85.                     InitTimer(0, TIME_BACK_AWAY);
  86.                     Reverse(MAX_SPEED);
  87.                     currentState = BACK;
  88.                     break;
  89.                 case FR_BUMP_DOWN:
  90.                     RotateLeft(MAX_SPEED);
  91.                     InitTimer(0, TIME_TURN_360 / 4);
  92.                     currentState = TURNLEFT;
  93.                     break;
  94.                 case ENTERED_DARK:
  95.                     StopTimer(0);
  96.                     ClearTimerExpired(0);
  97.                     Stop();
  98.                     currentState = DARKIDLE;
  99.                     break;
  100.             }
  101.             break;
  102.         case BACK:
  103.             switch (currentEvent) {
  104.                 case TIMER_0_EXPIRED:
  105.                     ClearTimerExpired(0);
  106.                     Forward(MAX_SPEED);
  107.                     currentState = FORWARD;
  108.                     break;
  109.                 case BACK_BUMP_DOWN:
  110.                     ArcRight(MAX_SPEED);
  111.                     InitTimer(0, 3000);
  112.                     currentState = ARCRIGHT;
  113.                     break;
  114.                 case ENTERED_DARK:
  115.                     StopTimer(0);
  116.                     ClearTimerExpired(0);
  117.                     Stop();
  118.                     currentState = DARKIDLE;
  119.                     break;
  120.             }
  121.             break;
  122.         case ARCRIGHT:
  123.             switch (currentEvent) {
  124.                 case TIMER_0_EXPIRED:
  125.                     ClearTimerExpired(0);
  126.                     Forward(MAX_SPEED);
  127.                     currentState = FORWARD;
  128.                     break;
  129.                 case FRONT_BUMP_DOWN:
  130.                     ClearTimerExpired(0);
  131.                     Reverse(MAX_SPEED);
  132.                     currentState = BACK;
  133.                     break;
  134.                 case FL_BUMP_DOWN:
  135.                     RotateRight(MAX_SPEED);
  136.                     InitTimer(0, TIME_TURN_360 / 4);
  137.                     currentState = TURNRIGHT;
  138.                     break;
  139.                 case FR_BUMP_DOWN:
  140.                     RotateLeft(MAX_SPEED);
  141.                     InitTimer(0, TIME_TURN_360 / 4);
  142.                     currentState = TURNLEFT;
  143.                     break;
  144.                 case ENTERED_DARK:
  145.                     StopTimer(0);
  146.                     ClearTimerExpired(0);
  147.                     Stop();
  148.                     currentState = DARKIDLE;
  149.                     break;
  150.             }
  151.             break;
  152.         case DARKIDLE:
  153.             switch (currentEvent) {
  154.                 case ENTERED_LIGHT:
  155.                     Forward(MAX_SPEED);
  156.                     currentState = FORWARD;
  157.                     break;
  158.             }
  159.             break;
  160.     }
  161. }
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!