asdf
Dependents: kinematics_controlv4 kinematics_control_copyfds Robot_control
Movement.cpp
- Committer:
- peterknoben
- Date:
- 2017-11-01
- Revision:
- 4:c81764efe40d
- Parent:
- 3:61733f7f1fea
File content as of revision 4:c81764efe40d:
#include "Movement.h" #include "mbed.h" float movement_left, movement_right; Movement::Movement(void) { } int Movement::getdirectionLeft(int mode){ int directionL; if(mode==1 || mode==2){ directionL = 1; } else if (mode==4 || mode==5){ directionL = -1; } else{ directionL = 0; } return directionL; } int Movement::getdirectionRight(int mode){ int directionR; if(mode==1 || mode==5){ directionR = 1; } else if (mode==2 || mode==4){ directionR = -1; } else{ directionR = 0; } return directionR; } //Get the left position float Movement::getpositionLeft(int SignalNumber, int mode, float max_range){ static float positionLeft; //Define a position array int directionLeft = getdirectionLeft(mode); float position_math_left; switch(SignalNumber){ case 1: //Move slowest movement_left = directionLeft*0.25; position_math_left = positionLeft + movement_left; if (position_math_left >= max_range || position_math_left <=0){ positionLeft = positionLeft; } else{ positionLeft += movement_left; } break; case 2: //Move slow movement_left = directionLeft *1; position_math_left = positionLeft + movement_left; if (position_math_left >= max_range || position_math_left <=0){ positionLeft = positionLeft; } else{ positionLeft += movement_left; } break; default : //if something is wrong or no muscles are flexed, standstill positionLeft = positionLeft; break; } return positionLeft; } // Get the right position float Movement::getpositionRight(int SignalNumber, int mode, float max_range){ static float positionRight; //Define a position array int directionRight = getdirectionRight(mode); float position_math_right; switch(SignalNumber){ case 1: //Move slowest movement_right = directionRight*0.25; position_math_right = positionRight + movement_right; if (position_math_right >= max_range || position_math_right <=0){ positionRight = positionRight; } else{ positionRight += movement_right; } break; case 2: //Move slow movement_right = directionRight *1; position_math_right = positionRight + movement_right; if (position_math_right >= max_range || position_math_right <=0){ positionRight = positionRight; } else{ positionRight += movement_right; } break; default : //if something is wrong or no muscles are flexed, standstill positionRight = positionRight; break; } return positionRight; }