asdf
Dependents: kinematics_controlv4 kinematics_control_copyfds Robot_control
Movement.cpp@2:43a6498000e8, 2017-11-01 (annotated)
- Committer:
- peterknoben
- Date:
- Wed Nov 01 13:42:11 2017 +0000
- Revision:
- 2:43a6498000e8
- Parent:
- 1:03e2651b4a24
- Child:
- 3:61733f7f1fea
sdf
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 | 0:9c61094ffaac | 4 | float movement; |
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 | 0:9c61094ffaac | 11 | int * Movement::getdirection(int mode){ |
peterknoben | 0:9c61094ffaac | 12 | //int *getdirection(int direction[2]){ |
peterknoben | 0:9c61094ffaac | 13 | static int direction[2]; |
peterknoben | 0:9c61094ffaac | 14 | if(mode==1){ |
peterknoben | 0:9c61094ffaac | 15 | direction[0]=1; |
peterknoben | 0:9c61094ffaac | 16 | direction[1]=1; |
peterknoben | 0:9c61094ffaac | 17 | } |
peterknoben | 0:9c61094ffaac | 18 | else if(mode==2){ |
peterknoben | 0:9c61094ffaac | 19 | direction[0]=1; |
peterknoben | 0:9c61094ffaac | 20 | direction[1]=-1; |
peterknoben | 0:9c61094ffaac | 21 | } |
peterknoben | 0:9c61094ffaac | 22 | else if(mode==3){ |
peterknoben | 1:03e2651b4a24 | 23 | direction[0]=0; |
peterknoben | 1:03e2651b4a24 | 24 | direction[1]=0; |
peterknoben | 0:9c61094ffaac | 25 | } |
peterknoben | 0:9c61094ffaac | 26 | else if(mode==4){ |
peterknoben | 0:9c61094ffaac | 27 | direction[0]=-1; |
peterknoben | 0:9c61094ffaac | 28 | direction[1]=-1; |
peterknoben | 0:9c61094ffaac | 29 | } |
peterknoben | 0:9c61094ffaac | 30 | else if(mode==5){ |
peterknoben | 0:9c61094ffaac | 31 | direction[0]=-1; |
peterknoben | 0:9c61094ffaac | 32 | direction[1]=1; |
peterknoben | 0:9c61094ffaac | 33 | } |
peterknoben | 0:9c61094ffaac | 34 | else if(mode==6){ |
peterknoben | 1:03e2651b4a24 | 35 | direction[0]=0; |
peterknoben | 1:03e2651b4a24 | 36 | direction[1]=0; |
peterknoben | 0:9c61094ffaac | 37 | } |
peterknoben | 0:9c61094ffaac | 38 | else{ |
peterknoben | 0:9c61094ffaac | 39 | //Error, unavailable mode |
peterknoben | 0:9c61094ffaac | 40 | } |
peterknoben | 0:9c61094ffaac | 41 | return direction; |
peterknoben | 0:9c61094ffaac | 42 | } |
peterknoben | 0:9c61094ffaac | 43 | |
peterknoben | 0:9c61094ffaac | 44 | |
peterknoben | 0:9c61094ffaac | 45 | float Movement::getposition(int SignalNumber, int mode, int side, float max_range){ |
peterknoben | 0:9c61094ffaac | 46 | static float position; //Define a position array |
peterknoben | 0:9c61094ffaac | 47 | int *direction_math; //Define a useable direction array in this function |
peterknoben | 0:9c61094ffaac | 48 | direction_math = getdirection(mode); //Fill the useable direction array |
peterknoben | 0:9c61094ffaac | 49 | float position_math; |
peterknoben | 0:9c61094ffaac | 50 | switch(SignalNumber){ |
peterknoben | 0:9c61094ffaac | 51 | case 1: //Move slowest |
peterknoben | 0:9c61094ffaac | 52 | movement = direction_math[side] *1; |
peterknoben | 0:9c61094ffaac | 53 | position_math = position + movement; |
peterknoben | 0:9c61094ffaac | 54 | if (position_math >= max_range && position_math <=0){ |
peterknoben | 0:9c61094ffaac | 55 | position = position; |
peterknoben | 0:9c61094ffaac | 56 | } |
peterknoben | 0:9c61094ffaac | 57 | else{ |
peterknoben | 0:9c61094ffaac | 58 | position += movement; |
peterknoben | 0:9c61094ffaac | 59 | } |
peterknoben | 0:9c61094ffaac | 60 | break; |
peterknoben | 0:9c61094ffaac | 61 | case 2: //Move slow |
peterknoben | 0:9c61094ffaac | 62 | movement = direction_math[side] *2; |
peterknoben | 0:9c61094ffaac | 63 | position_math = position + movement; |
peterknoben | 0:9c61094ffaac | 64 | if (position_math >= max_range && position_math <=0){ |
peterknoben | 0:9c61094ffaac | 65 | position = position; |
peterknoben | 0:9c61094ffaac | 66 | } |
peterknoben | 0:9c61094ffaac | 67 | else{ |
peterknoben | 0:9c61094ffaac | 68 | position += movement; |
peterknoben | 0:9c61094ffaac | 69 | } |
peterknoben | 0:9c61094ffaac | 70 | break; |
peterknoben | 0:9c61094ffaac | 71 | case 3: //Move fast |
peterknoben | 0:9c61094ffaac | 72 | movement = direction_math[side] *5; |
peterknoben | 0:9c61094ffaac | 73 | position_math = position + movement; |
peterknoben | 0:9c61094ffaac | 74 | if (position_math >= max_range && position_math <=0){ |
peterknoben | 0:9c61094ffaac | 75 | position = position; |
peterknoben | 0:9c61094ffaac | 76 | } |
peterknoben | 0:9c61094ffaac | 77 | else{ |
peterknoben | 0:9c61094ffaac | 78 | position += movement; |
peterknoben | 0:9c61094ffaac | 79 | } |
peterknoben | 0:9c61094ffaac | 80 | break; |
peterknoben | 0:9c61094ffaac | 81 | case 4: //Move fastest |
peterknoben | 0:9c61094ffaac | 82 | movement = direction_math[side] *10; |
peterknoben | 0:9c61094ffaac | 83 | position_math = position + movement; |
peterknoben | 0:9c61094ffaac | 84 | if (position_math >= max_range && position_math <=0){ |
peterknoben | 0:9c61094ffaac | 85 | position = position; |
peterknoben | 0:9c61094ffaac | 86 | } |
peterknoben | 0:9c61094ffaac | 87 | else{ |
peterknoben | 0:9c61094ffaac | 88 | position += movement; |
peterknoben | 0:9c61094ffaac | 89 | } |
peterknoben | 0:9c61094ffaac | 90 | break; |
peterknoben | 0:9c61094ffaac | 91 | default : //if something is wrong or no muscles are flexed, standstill |
peterknoben | 0:9c61094ffaac | 92 | position = position; |
peterknoben | 0:9c61094ffaac | 93 | break; |
peterknoben | 0:9c61094ffaac | 94 | } |
peterknoben | 0:9c61094ffaac | 95 | return position; |
peterknoben | 0:9c61094ffaac | 96 | } |
peterknoben | 0:9c61094ffaac | 97 |