asdf
Dependents: kinematics_controlv4 kinematics_control_copyfds Robot_control
Movement.cpp@0:9c61094ffaac, 2017-10-31 (annotated)
- Committer:
- peterknoben
- Date:
- Tue Oct 31 14:34:37 2017 +0000
- Revision:
- 0:9c61094ffaac
- Child:
- 1:03e2651b4a24
fsdaf
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 | 0:9c61094ffaac | 23 | //servo |
peterknoben | 0:9c61094ffaac | 24 | } |
peterknoben | 0:9c61094ffaac | 25 | else if(mode==4){ |
peterknoben | 0:9c61094ffaac | 26 | direction[0]=-1; |
peterknoben | 0:9c61094ffaac | 27 | direction[1]=-1; |
peterknoben | 0:9c61094ffaac | 28 | } |
peterknoben | 0:9c61094ffaac | 29 | else if(mode==5){ |
peterknoben | 0:9c61094ffaac | 30 | direction[0]=-1; |
peterknoben | 0:9c61094ffaac | 31 | direction[1]=1; |
peterknoben | 0:9c61094ffaac | 32 | } |
peterknoben | 0:9c61094ffaac | 33 | else if(mode==6){ |
peterknoben | 0:9c61094ffaac | 34 | //servo |
peterknoben | 0:9c61094ffaac | 35 | } |
peterknoben | 0:9c61094ffaac | 36 | else{ |
peterknoben | 0:9c61094ffaac | 37 | //Error, unavailable mode |
peterknoben | 0:9c61094ffaac | 38 | } |
peterknoben | 0:9c61094ffaac | 39 | return direction; |
peterknoben | 0:9c61094ffaac | 40 | } |
peterknoben | 0:9c61094ffaac | 41 | |
peterknoben | 0:9c61094ffaac | 42 | |
peterknoben | 0:9c61094ffaac | 43 | float Movement::getposition(int SignalNumber, int mode, int side, float max_range){ |
peterknoben | 0:9c61094ffaac | 44 | static float position; //Define a position array |
peterknoben | 0:9c61094ffaac | 45 | int *direction_math; //Define a useable direction array in this function |
peterknoben | 0:9c61094ffaac | 46 | direction_math = getdirection(mode); //Fill the useable direction array |
peterknoben | 0:9c61094ffaac | 47 | float position_math; |
peterknoben | 0:9c61094ffaac | 48 | switch(SignalNumber){ |
peterknoben | 0:9c61094ffaac | 49 | case 1: //Move slowest |
peterknoben | 0:9c61094ffaac | 50 | movement = direction_math[side] *1; |
peterknoben | 0:9c61094ffaac | 51 | position_math = position + movement; |
peterknoben | 0:9c61094ffaac | 52 | if (position_math >= max_range && position_math <=0){ |
peterknoben | 0:9c61094ffaac | 53 | position = position; |
peterknoben | 0:9c61094ffaac | 54 | } |
peterknoben | 0:9c61094ffaac | 55 | else{ |
peterknoben | 0:9c61094ffaac | 56 | position += movement; |
peterknoben | 0:9c61094ffaac | 57 | } |
peterknoben | 0:9c61094ffaac | 58 | break; |
peterknoben | 0:9c61094ffaac | 59 | case 2: //Move slow |
peterknoben | 0:9c61094ffaac | 60 | movement = direction_math[side] *2; |
peterknoben | 0:9c61094ffaac | 61 | position_math = position + movement; |
peterknoben | 0:9c61094ffaac | 62 | if (position_math >= max_range && position_math <=0){ |
peterknoben | 0:9c61094ffaac | 63 | position = position; |
peterknoben | 0:9c61094ffaac | 64 | } |
peterknoben | 0:9c61094ffaac | 65 | else{ |
peterknoben | 0:9c61094ffaac | 66 | position += movement; |
peterknoben | 0:9c61094ffaac | 67 | } |
peterknoben | 0:9c61094ffaac | 68 | break; |
peterknoben | 0:9c61094ffaac | 69 | case 3: //Move fast |
peterknoben | 0:9c61094ffaac | 70 | movement = direction_math[side] *5; |
peterknoben | 0:9c61094ffaac | 71 | position_math = position + movement; |
peterknoben | 0:9c61094ffaac | 72 | if (position_math >= max_range && position_math <=0){ |
peterknoben | 0:9c61094ffaac | 73 | position = position; |
peterknoben | 0:9c61094ffaac | 74 | } |
peterknoben | 0:9c61094ffaac | 75 | else{ |
peterknoben | 0:9c61094ffaac | 76 | position += movement; |
peterknoben | 0:9c61094ffaac | 77 | } |
peterknoben | 0:9c61094ffaac | 78 | break; |
peterknoben | 0:9c61094ffaac | 79 | case 4: //Move fastest |
peterknoben | 0:9c61094ffaac | 80 | movement = direction_math[side] *10; |
peterknoben | 0:9c61094ffaac | 81 | position_math = position + movement; |
peterknoben | 0:9c61094ffaac | 82 | if (position_math >= max_range && position_math <=0){ |
peterknoben | 0:9c61094ffaac | 83 | position = position; |
peterknoben | 0:9c61094ffaac | 84 | } |
peterknoben | 0:9c61094ffaac | 85 | else{ |
peterknoben | 0:9c61094ffaac | 86 | position += movement; |
peterknoben | 0:9c61094ffaac | 87 | } |
peterknoben | 0:9c61094ffaac | 88 | break; |
peterknoben | 0:9c61094ffaac | 89 | default : //if something is wrong or no muscles are flexed, standstill |
peterknoben | 0:9c61094ffaac | 90 | position = position; |
peterknoben | 0:9c61094ffaac | 91 | break; |
peterknoben | 0:9c61094ffaac | 92 | } |
peterknoben | 0:9c61094ffaac | 93 | return position; |
peterknoben | 0:9c61094ffaac | 94 | } |
peterknoben | 0:9c61094ffaac | 95 | |
peterknoben | 0:9c61094ffaac | 96 | |
peterknoben | 0:9c61094ffaac | 97 | |
peterknoben | 0:9c61094ffaac | 98 | |
peterknoben | 0:9c61094ffaac | 99 | |
peterknoben | 0:9c61094ffaac | 100 | /* |
peterknoben | 0:9c61094ffaac | 101 | float *Movement::getposition(int SignalNumber, int mode){ |
peterknoben | 0:9c61094ffaac | 102 | static float position[2]; //Define a position array |
peterknoben | 0:9c61094ffaac | 103 | int *direction_math; //Define a useable direction array in this function |
peterknoben | 0:9c61094ffaac | 104 | direction_math = getdirection(mode); //Fill the useable direction array |
peterknoben | 0:9c61094ffaac | 105 | switch(SignalNumber){ |
peterknoben | 0:9c61094ffaac | 106 | case 1: //Move slowest |
peterknoben | 0:9c61094ffaac | 107 | xmovement = direction_math[0] * 1; //mm |
peterknoben | 0:9c61094ffaac | 108 | ymovement = direction_math[1] * 1; //mm |
peterknoben | 0:9c61094ffaac | 109 | position[0] += xmovement; |
peterknoben | 0:9c61094ffaac | 110 | position[1] += ymovement; |
peterknoben | 0:9c61094ffaac | 111 | break; |
peterknoben | 0:9c61094ffaac | 112 | case 2: //Move slow |
peterknoben | 0:9c61094ffaac | 113 | xmovement = direction_math[0] * 2; //mm |
peterknoben | 0:9c61094ffaac | 114 | ymovement = direction_math[1] * 2; //mm |
peterknoben | 0:9c61094ffaac | 115 | position[0] += xmovement; |
peterknoben | 0:9c61094ffaac | 116 | position[1] += ymovement; |
peterknoben | 0:9c61094ffaac | 117 | break; |
peterknoben | 0:9c61094ffaac | 118 | case 3: //Move fast |
peterknoben | 0:9c61094ffaac | 119 | xmovement = direction_math[0] * 5; //mm |
peterknoben | 0:9c61094ffaac | 120 | ymovement = direction_math[1] * 5; //mm |
peterknoben | 0:9c61094ffaac | 121 | position[0] += xmovement; |
peterknoben | 0:9c61094ffaac | 122 | position[1] += ymovement; |
peterknoben | 0:9c61094ffaac | 123 | break; |
peterknoben | 0:9c61094ffaac | 124 | case 4: //Move fastest |
peterknoben | 0:9c61094ffaac | 125 | xmovement = direction_math[0] * 10; //mm |
peterknoben | 0:9c61094ffaac | 126 | ymovement = direction_math[1] * 10; //mm |
peterknoben | 0:9c61094ffaac | 127 | position[0] += xmovement; |
peterknoben | 0:9c61094ffaac | 128 | position[1] += ymovement; |
peterknoben | 0:9c61094ffaac | 129 | break; |
peterknoben | 0:9c61094ffaac | 130 | default : //if something is wrong or no muscles are flexed, standstill |
peterknoben | 0:9c61094ffaac | 131 | position[0] = position[0]; |
peterknoben | 0:9c61094ffaac | 132 | position[1] = position[1]; |
peterknoben | 0:9c61094ffaac | 133 | break; |
peterknoben | 0:9c61094ffaac | 134 | } |
peterknoben | 0:9c61094ffaac | 135 | return position; |
peterknoben | 0:9c61094ffaac | 136 | } |
peterknoben | 0:9c61094ffaac | 137 | */ |
peterknoben | 0:9c61094ffaac | 138 |