linearActuatorControl.cpp

00001 #include "linearActuatorControl.h"
00002 
00003 LinearActuatorControl::LinearActuatorControl() : _controlMode(D7), _runMode(D8), 
00004     _linAct1(D9), _linAct2(D10), _linAct3(D11), _linAct4(D12), 
00005     _actBtn1(D3), _actBtn2(D4), _actBtn3(D5), _actBtn4(D6) {
00006         
00007     for (int i = 0; i < 4; i++) {
00008         _idlePositions[i] = 0;
00009         _actPositions[i] = 0;
00010         _manualPositionsDown[i] = 0;
00011         _manualPositionsUp[i] = 1;
00012     }
00013 
00014     _linAct1.period_ms(20);
00015     _linAct2.period_ms(20);
00016     _linAct3.period_ms(20);
00017     _linAct4.period_ms(20);
00018 }
00019 
00020 void LinearActuatorControl::refresh() {
00021     if (!_controlMode) {
00022         _linAct1.pulsewidth_us(!_actBtn1 ? (1000*(1+_manualPositionsDown[0])) : (1000*(1+_manualPositionsUp[0])));
00023         _linAct2.pulsewidth_us(!_actBtn2 ? (1000*(1+_manualPositionsDown[1])) : (1000*(1+_manualPositionsUp[1])));
00024         _linAct3.pulsewidth_us(!_actBtn3 ? (1000*(1+_manualPositionsDown[2])) : (1000*(1+_manualPositionsUp[2])));
00025         _linAct4.pulsewidth_us(!_actBtn4 ? (1000*(1+_manualPositionsDown[3])) : (1000*(1+_manualPositionsUp[3])));
00026         
00027     } else {
00028         if (_runMode) { 
00029             //update PWM to sequence positions
00030             _linAct1.pulsewidth_us(1000*(1+_actPositions[0]));
00031             _linAct2.pulsewidth_us(1000*(1+_actPositions[1]));
00032             _linAct3.pulsewidth_us(1000*(1+_actPositions[2]));
00033             _linAct4.pulsewidth_us(1000*(1+_actPositions[3]));
00034         } else {
00035             //change to idle positions
00036             _linAct1.pulsewidth_us(1000*(1+_idlePositions[0]));
00037             _linAct2.pulsewidth_us(1000*(1+_idlePositions[1]));
00038             _linAct3.pulsewidth_us(1000*(1+_idlePositions[2]));
00039             _linAct4.pulsewidth_us(1000*(1+_idlePositions[3]));
00040         }
00041     }
00042 }
00043 
00044 void LinearActuatorControl::setIdlePositions(float pos1, float pos2, float pos3, float pos4) {
00045     _idlePositions[0] = pos1;
00046     _idlePositions[1] = pos2;
00047     _idlePositions[2] = pos3;
00048     _idlePositions[3] = pos4;
00049 }
00050 
00051 void LinearActuatorControl::setManualPositionsDown(float pos1, float pos2, float pos3, float pos4) {
00052     _manualPositionsDown[0] = pos1;
00053     _manualPositionsDown[1] = pos2;
00054     _manualPositionsDown[2] = pos3;
00055     _manualPositionsDown[3] = pos4;
00056 }
00057 
00058 void LinearActuatorControl::setManualPositionsUp(float pos1, float pos2, float pos3, float pos4) {
00059     _manualPositionsUp[0] = pos1;
00060     _manualPositionsUp[1] = pos2;
00061     _manualPositionsUp[2] = pos3;
00062     _manualPositionsUp[3] = pos4;
00063 }
00064 
00065 void LinearActuatorControl::move(float pos1, float pos2, float pos3, float pos4) {
00066     _actPositions[0] = pos1;
00067     _actPositions[1] = pos2;
00068     _actPositions[2] = pos3;
00069     _actPositions[3] = pos4;
00070     refresh();
00071 }
00072 
00073 bool LinearActuatorControl::inUserSequence() {
00074     if (_controlMode && _runMode) {
00075         return true;
00076     } 
00077     return false;
00078 }
00079 
00080 float _convertToPulseWidth_us(float input) {
00081     return 1000*(1+input);   
00082 }