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