Dustin Berendsen
Eindelijk!!!!!
Fork of
Movement
by 
Peter Knoben
Movement.cpp@3:61733f7f1fea, 2017-11-01 (annotated)
- Committer:
- peterknoben
- Date:
- Wed Nov 01 22:23:17 2017 +0000
- Revision:
- 3:61733f7f1fea
- Parent:
- 2:43a6498000e8
- Child:
- 4:c81764efe40d
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 | 3:61733f7f1fea | 40 | float Movement::getpositionLeft(int SignalNumber, int mode, float max_range){ |
| peterknoben | 3:61733f7f1fea | 41 | static float positionLeft; //Define a position array |
| peterknoben | 3:61733f7f1fea | 42 | int directionLeft = getdirectionLeft(mode); |
| peterknoben | 3:61733f7f1fea | 43 | float position_math_left; |
| peterknoben | 3:61733f7f1fea | 44 | switch(SignalNumber){ |
| peterknoben | 3:61733f7f1fea | 45 | case 1: //Move slowest |
| peterknoben | 3:61733f7f1fea | 46 | movement_left = directionLeft*0.25; |
| peterknoben | 3:61733f7f1fea | 47 | position_math_left = positionLeft + movement_left; |
| peterknoben | 3:61733f7f1fea | 48 | if (position_math_left >= max_range || position_math_left <=0){ |
| peterknoben | 3:61733f7f1fea | 49 | positionLeft = positionLeft; |
| peterknoben | 3:61733f7f1fea | 50 | } |
| peterknoben | 3:61733f7f1fea | 51 | else{ |
| peterknoben | 3:61733f7f1fea | 52 | positionLeft += movement_left; |
| peterknoben | 3:61733f7f1fea | 53 | } |
| peterknoben | 3:61733f7f1fea | 54 | break; |
| peterknoben | 3:61733f7f1fea | 55 | case 2: //Move slow |
| peterknoben | 3:61733f7f1fea | 56 | movement_left = directionLeft *1; |
| peterknoben | 3:61733f7f1fea | 57 | position_math_left = positionLeft + movement_left; |
| peterknoben | 3:61733f7f1fea | 58 | if (position_math_left >= max_range || position_math_left <=0){ |
| peterknoben | 3:61733f7f1fea | 59 | positionLeft = positionLeft; |
| peterknoben | 3:61733f7f1fea | 60 | } |
| peterknoben | 3:61733f7f1fea | 61 | else{ |
| peterknoben | 3:61733f7f1fea | 62 | positionLeft += movement_left; |
| peterknoben | 3:61733f7f1fea | 63 | } |
| peterknoben | 3:61733f7f1fea | 64 | break; |
| peterknoben | 3:61733f7f1fea | 65 | default : //if something is wrong or no muscles are flexed, standstill |
| peterknoben | 3:61733f7f1fea | 66 | positionLeft = positionLeft; |
| peterknoben | 3:61733f7f1fea | 67 | break; |
| peterknoben | 3:61733f7f1fea | 68 | } |
| peterknoben | 3:61733f7f1fea | 69 | return positionLeft; |
| peterknoben | 3:61733f7f1fea | 70 | } |
| peterknoben | 3:61733f7f1fea | 71 | |
| peterknoben | 3:61733f7f1fea | 72 | float Movement::getpositionRight(int SignalNumber, int mode, float max_range){ |
| peterknoben | 3:61733f7f1fea | 73 | static float positionRight; //Define a position array |
| peterknoben | 3:61733f7f1fea | 74 | int directionRight = getdirectionRight(mode); |
| peterknoben | 3:61733f7f1fea | 75 | float position_math_right; |
| peterknoben | 3:61733f7f1fea | 76 | switch(SignalNumber){ |
| peterknoben | 3:61733f7f1fea | 77 | case 1: //Move slowest |
| peterknoben | 3:61733f7f1fea | 78 | movement_right = directionRight*0.25; |
| peterknoben | 3:61733f7f1fea | 79 | position_math_right = positionRight + movement_right; |
| peterknoben | 3:61733f7f1fea | 80 | if (position_math_right >= max_range || position_math_right <=0){ |
| peterknoben | 3:61733f7f1fea | 81 | positionRight = positionRight; |
| peterknoben | 3:61733f7f1fea | 82 | } |
| peterknoben | 3:61733f7f1fea | 83 | else{ |
| peterknoben | 3:61733f7f1fea | 84 | positionRight += movement_right; |
| peterknoben | 3:61733f7f1fea | 85 | } |
| peterknoben | 3:61733f7f1fea | 86 | break; |
| peterknoben | 3:61733f7f1fea | 87 | case 2: //Move slow |
| peterknoben | 3:61733f7f1fea | 88 | movement_right = directionRight *1; |
| peterknoben | 3:61733f7f1fea | 89 | position_math_right = positionRight + movement_right; |
| peterknoben | 3:61733f7f1fea | 90 | if (position_math_right >= max_range || position_math_right <=0){ |
| peterknoben | 3:61733f7f1fea | 91 | positionRight = positionRight; |
| peterknoben | 3:61733f7f1fea | 92 | } |
| peterknoben | 3:61733f7f1fea | 93 | else{ |
| peterknoben | 3:61733f7f1fea | 94 | positionRight += movement_right; |
| peterknoben | 3:61733f7f1fea | 95 | } |
| peterknoben | 3:61733f7f1fea | 96 | break; |
| peterknoben | 3:61733f7f1fea | 97 | default : //if something is wrong or no muscles are flexed, standstill |
| peterknoben | 3:61733f7f1fea | 98 | positionRight = positionRight; |
| peterknoben | 3:61733f7f1fea | 99 | break; |
| peterknoben | 3:61733f7f1fea | 100 | } |
| peterknoben | 3:61733f7f1fea | 101 | return positionRight; |
| peterknoben | 3:61733f7f1fea | 102 | } |
| peterknoben | 3:61733f7f1fea | 103 | |
| peterknoben | 3:61733f7f1fea | 104 | |
| peterknoben | 3:61733f7f1fea | 105 | /* |
| peterknoben | 3:61733f7f1fea | 106 | |
| peterknoben | 0:9c61094ffaac | 107 | int * Movement::getdirection(int mode){ |
| peterknoben | 0:9c61094ffaac | 108 | //int *getdirection(int direction[2]){ |
| peterknoben | 0:9c61094ffaac | 109 | static int direction[2]; |
| peterknoben | 0:9c61094ffaac | 110 | if(mode==1){ |
| peterknoben | 0:9c61094ffaac | 111 | direction[0]=1; |
| peterknoben | 0:9c61094ffaac | 112 | direction[1]=1; |
| peterknoben | 0:9c61094ffaac | 113 | } |
| peterknoben | 0:9c61094ffaac | 114 | else if(mode==2){ |
| peterknoben | 0:9c61094ffaac | 115 | direction[0]=1; |
| peterknoben | 0:9c61094ffaac | 116 | direction[1]=-1; |
| peterknoben | 0:9c61094ffaac | 117 | } |
| peterknoben | 0:9c61094ffaac | 118 | else if(mode==3){ |
| peterknoben | 1:03e2651b4a24 | 119 | direction[0]=0; |
| peterknoben | 1:03e2651b4a24 | 120 | direction[1]=0; |
| peterknoben | 0:9c61094ffaac | 121 | } |
| peterknoben | 0:9c61094ffaac | 122 | else if(mode==4){ |
| peterknoben | 0:9c61094ffaac | 123 | direction[0]=-1; |
| peterknoben | 0:9c61094ffaac | 124 | direction[1]=-1; |
| peterknoben | 0:9c61094ffaac | 125 | } |
| peterknoben | 0:9c61094ffaac | 126 | else if(mode==5){ |
| peterknoben | 0:9c61094ffaac | 127 | direction[0]=-1; |
| peterknoben | 0:9c61094ffaac | 128 | direction[1]=1; |
| peterknoben | 0:9c61094ffaac | 129 | } |
| peterknoben | 0:9c61094ffaac | 130 | else if(mode==6){ |
| peterknoben | 1:03e2651b4a24 | 131 | direction[0]=0; |
| peterknoben | 1:03e2651b4a24 | 132 | direction[1]=0; |
| peterknoben | 0:9c61094ffaac | 133 | } |
| peterknoben | 0:9c61094ffaac | 134 | else{ |
| peterknoben | 0:9c61094ffaac | 135 | //Error, unavailable mode |
| peterknoben | 0:9c61094ffaac | 136 | } |
| peterknoben | 0:9c61094ffaac | 137 | return direction; |
| peterknoben | 0:9c61094ffaac | 138 | } |
| peterknoben | 3:61733f7f1fea | 139 | */ |