Peter Knoben
Working, Clean
Fork of
Movement
by 
Dustin Berendsen
Movement.cpp@6:e21a1de9bb8c, 2017-11-02 (annotated)
- Committer:
- peterknoben
- Date:
- Thu Nov 02 15:08:14 2017 +0000
- Revision:
- 6:e21a1de9bb8c
- Parent:
- 5:d13de19a1eea
Working
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| peterknoben | 0:9c61094ffaac | 1 | #include "Movement.h" |
| peterknoben | 0:9c61094ffaac | 2 | #include "mbed.h" |
| peterknoben | 0:9c61094ffaac | 3 | |
| peterknoben | 3:61733f7f1fea | 4 | float movement_left, movement_right; |
| peterknoben | 0:9c61094ffaac | 5 | |
| peterknoben | 0:9c61094ffaac | 6 | Movement::Movement(void) |
| peterknoben | 0:9c61094ffaac | 7 | { |
| peterknoben | 0:9c61094ffaac | 8 | |
| peterknoben | 0:9c61094ffaac | 9 | } |
| peterknoben | 0:9c61094ffaac | 10 | |
| peterknoben | 6:e21a1de9bb8c | 11 | //------------------------------------------------------------------------------ |
| peterknoben | 3:61733f7f1fea | 12 | int Movement::getdirectionLeft(int mode){ |
| peterknoben | 3:61733f7f1fea | 13 | int directionL; |
| peterknoben | 3:61733f7f1fea | 14 | if(mode==1 || mode==2){ |
| peterknoben | 3:61733f7f1fea | 15 | directionL = 1; |
| peterknoben | 3:61733f7f1fea | 16 | } |
| peterknoben | 3:61733f7f1fea | 17 | else if (mode==4 || mode==5){ |
| peterknoben | 3:61733f7f1fea | 18 | directionL = -1; |
| peterknoben | 3:61733f7f1fea | 19 | } |
| peterknoben | 3:61733f7f1fea | 20 | else{ |
| peterknoben | 3:61733f7f1fea | 21 | directionL = 0; |
| peterknoben | 3:61733f7f1fea | 22 | } |
| peterknoben | 3:61733f7f1fea | 23 | return directionL; |
| peterknoben | 3:61733f7f1fea | 24 | } |
| peterknoben | 3:61733f7f1fea | 25 | |
| peterknoben | 6:e21a1de9bb8c | 26 | //------------------------------------------------------------------------------ |
| peterknoben | 3:61733f7f1fea | 27 | int Movement::getdirectionRight(int mode){ |
| peterknoben | 3:61733f7f1fea | 28 | int directionR; |
| peterknoben | 3:61733f7f1fea | 29 | if(mode==1 || mode==5){ |
| peterknoben | 3:61733f7f1fea | 30 | directionR = 1; |
| peterknoben | 3:61733f7f1fea | 31 | } |
| peterknoben | 3:61733f7f1fea | 32 | else if (mode==2 || mode==4){ |
| peterknoben | 3:61733f7f1fea | 33 | directionR = -1; |
| peterknoben | 3:61733f7f1fea | 34 | } |
| peterknoben | 3:61733f7f1fea | 35 | else{ |
| peterknoben | 3:61733f7f1fea | 36 | directionR = 0; |
| peterknoben | 3:61733f7f1fea | 37 | } |
| peterknoben | 3:61733f7f1fea | 38 | return directionR; |
| peterknoben | 3:61733f7f1fea | 39 | } |
| peterknoben | 3:61733f7f1fea | 40 | |
| peterknoben | 6:e21a1de9bb8c | 41 | //------------------------------------------------------------------------------ |
| peterknoben | 4:c81764efe40d | 42 | //Get the left position |
| peterknoben | 3:61733f7f1fea | 43 | float Movement::getpositionLeft(int SignalNumber, int mode, float max_range){ |
| peterknoben | 3:61733f7f1fea | 44 | static float positionLeft; //Define a position array |
| peterknoben | 3:61733f7f1fea | 45 | int directionLeft = getdirectionLeft(mode); |
| peterknoben | 3:61733f7f1fea | 46 | float position_math_left; |
| peterknoben | 3:61733f7f1fea | 47 | switch(SignalNumber){ |
| peterknoben | 3:61733f7f1fea | 48 | case 1: //Move slowest |
| peterknoben | 6:e21a1de9bb8c | 49 | movement_left = directionLeft*0.25; |
| peterknoben | 3:61733f7f1fea | 50 | position_math_left = positionLeft + movement_left; |
| peterknoben | 3:61733f7f1fea | 51 | if (position_math_left >= max_range || position_math_left <=0){ |
| peterknoben | 3:61733f7f1fea | 52 | positionLeft = positionLeft; |
| peterknoben | 3:61733f7f1fea | 53 | } |
| peterknoben | 3:61733f7f1fea | 54 | else{ |
| peterknoben | 3:61733f7f1fea | 55 | positionLeft += movement_left; |
| peterknoben | 3:61733f7f1fea | 56 | } |
| peterknoben | 3:61733f7f1fea | 57 | break; |
| peterknoben | 3:61733f7f1fea | 58 | case 2: //Move slow |
| peterknoben | 6:e21a1de9bb8c | 59 | movement_left = directionLeft *1; |
| peterknoben | 3:61733f7f1fea | 60 | position_math_left = positionLeft + movement_left; |
| peterknoben | 3:61733f7f1fea | 61 | if (position_math_left >= max_range || position_math_left <=0){ |
| peterknoben | 3:61733f7f1fea | 62 | positionLeft = positionLeft; |
| peterknoben | 3:61733f7f1fea | 63 | } |
| peterknoben | 3:61733f7f1fea | 64 | else{ |
| peterknoben | 3:61733f7f1fea | 65 | positionLeft += movement_left; |
| peterknoben | 3:61733f7f1fea | 66 | } |
| peterknoben | 3:61733f7f1fea | 67 | break; |
| peterknoben | 3:61733f7f1fea | 68 | default : //if something is wrong or no muscles are flexed, standstill |
| peterknoben | 3:61733f7f1fea | 69 | positionLeft = positionLeft; |
| peterknoben | 3:61733f7f1fea | 70 | break; |
| peterknoben | 3:61733f7f1fea | 71 | } |
| peterknoben | 3:61733f7f1fea | 72 | return positionLeft; |
| peterknoben | 3:61733f7f1fea | 73 | } |
| peterknoben | 3:61733f7f1fea | 74 | |
| peterknoben | 6:e21a1de9bb8c | 75 | //------------------------------------------------------------------------------ |
| peterknoben | 4:c81764efe40d | 76 | // Get the right position |
| peterknoben | 3:61733f7f1fea | 77 | float Movement::getpositionRight(int SignalNumber, int mode, float max_range){ |
| peterknoben | 3:61733f7f1fea | 78 | static float positionRight; //Define a position array |
| peterknoben | 3:61733f7f1fea | 79 | int directionRight = getdirectionRight(mode); |
| peterknoben | 3:61733f7f1fea | 80 | float position_math_right; |
| peterknoben | 3:61733f7f1fea | 81 | switch(SignalNumber){ |
| peterknoben | 3:61733f7f1fea | 82 | case 1: //Move slowest |
| peterknoben | 3:61733f7f1fea | 83 | movement_right = directionRight*0.25; |
| peterknoben | 3:61733f7f1fea | 84 | position_math_right = positionRight + movement_right; |
| peterknoben | 3:61733f7f1fea | 85 | if (position_math_right >= max_range || position_math_right <=0){ |
| peterknoben | 3:61733f7f1fea | 86 | positionRight = positionRight; |
| peterknoben | 3:61733f7f1fea | 87 | } |
| peterknoben | 3:61733f7f1fea | 88 | else{ |
| peterknoben | 3:61733f7f1fea | 89 | positionRight += movement_right; |
| peterknoben | 3:61733f7f1fea | 90 | } |
| peterknoben | 3:61733f7f1fea | 91 | break; |
| peterknoben | 3:61733f7f1fea | 92 | case 2: //Move slow |
| peterknoben | 3:61733f7f1fea | 93 | movement_right = directionRight *1; |
| peterknoben | 3:61733f7f1fea | 94 | position_math_right = positionRight + movement_right; |
| peterknoben | 3:61733f7f1fea | 95 | if (position_math_right >= max_range || position_math_right <=0){ |
| peterknoben | 3:61733f7f1fea | 96 | positionRight = positionRight; |
| peterknoben | 3:61733f7f1fea | 97 | } |
| peterknoben | 3:61733f7f1fea | 98 | else{ |
| peterknoben | 3:61733f7f1fea | 99 | positionRight += movement_right; |
| peterknoben | 3:61733f7f1fea | 100 | } |
| peterknoben | 3:61733f7f1fea | 101 | break; |
| peterknoben | 3:61733f7f1fea | 102 | default : //if something is wrong or no muscles are flexed, standstill |
| peterknoben | 3:61733f7f1fea | 103 | positionRight = positionRight; |
| peterknoben | 3:61733f7f1fea | 104 | break; |
| peterknoben | 3:61733f7f1fea | 105 | } |
| peterknoben | 3:61733f7f1fea | 106 | return positionRight; |
| peterknoben | 6:e21a1de9bb8c | 107 | } |