2018-12-21 3
Dependents: mbed-os-rest-api-V1-2 mbed-os-rest-api-V1-2-2
Diff: WheelManager.cpp
- Revision:
- 0:61923a9dd110
- Child:
- 1:b794ac1a3aa6
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/WheelManager.cpp Fri Dec 21 07:54:10 2018 +0000 @@ -0,0 +1,348 @@ +#include "module_for_all.h" + +/*DigitalOut _W1_A(PC_6); +DigitalOut _W1_B(PD_8); + +DigitalOut _W2_A(PA_12); +DigitalOut _W2_B(PA_11); + +DigitalOut _W3_A(PB_12); +DigitalOut _W3_B(PB_11); + +DigitalOut _W4_A(PB_2); +DigitalOut _W4_B(PB_15);*/ + + +WheelManager::WheelManager() { + DataStorage::_W1_A = 0; + DataStorage::_W1_B = 0; + + DataStorage::_W2_A = 0; + DataStorage::_W2_B = 0; + + DataStorage::_W3_A = 0; + DataStorage::_W3_B = 0; + + DataStorage::_W4_A = 0; + DataStorage::_W4_B = 0; +} + +WheelManager::WheelManager(std::string tmpTest) { + DataStorage::_W1_A = 0; + DataStorage::_W1_B = 0; + + DataStorage::_W2_A = 0; + DataStorage::_W2_B = 0; + + DataStorage::_W3_A = 0; + DataStorage::_W3_B = 0; + + DataStorage::_W4_A = 0; + DataStorage::_W4_B = 0; +} + +void WheelManager::moveDirectionControl(int tmpMoveDirection) { + _moveDirection = tmpMoveDirection; + + //W1 W4// + if (tmpMoveDirection>=5 && tmpMoveDirection<=9 || tmpMoveDirection==3 || tmpMoveDirection==12){ + DataStorage::_W1_A = 0; + DataStorage::_W1_B = 1; + + DataStorage::_W4_A = 1; + DataStorage::_W4_B = 0; + } else if (tmpMoveDirection>=13 && tmpMoveDirection<=15 || tmpMoveDirection==0 || tmpMoveDirection==1 || tmpMoveDirection==4 || tmpMoveDirection==11){ + DataStorage::_W1_A = 1; + DataStorage::_W1_B = 0; + + DataStorage::_W4_A = 0; + DataStorage::_W4_B = 1; + } else if (tmpMoveDirection==2 || tmpMoveDirection==10){ + DataStorage::_W1_A = 0; + DataStorage::_W1_B = 0; + + DataStorage::_W4_A = 0; + DataStorage::_W4_B = 0; + } + + //W2 W3// + if (tmpMoveDirection>=0 && tmpMoveDirection<=3 || tmpMoveDirection==5 || tmpMoveDirection==12 || tmpMoveDirection==15){ + DataStorage::_W2_A = 0; + DataStorage::_W2_B = 1; + + DataStorage::_W3_A = 1; + DataStorage::_W3_B = 0; + } else if (tmpMoveDirection>=7 && tmpMoveDirection<=11 || tmpMoveDirection==4 || tmpMoveDirection==13){ + DataStorage::_W2_A = 1; + DataStorage::_W2_B = 0; + + DataStorage::_W3_A = 0; + DataStorage::_W3_B = 1; + } else if (tmpMoveDirection==6 || tmpMoveDirection==14){ + DataStorage::_W2_A = 0; + DataStorage::_W2_B = 0; + + DataStorage::_W3_A = 0; + DataStorage::_W3_B = 0; + } + + //W1 W2 W3 W4// + if (tmpMoveDirection==16){ + DataStorage::_W1_A = 1; + DataStorage::_W1_B = 0; + + DataStorage::_W2_A = 1; + DataStorage::_W2_B = 0; + + DataStorage::_W3_A = 1; + DataStorage::_W3_B = 0; + + DataStorage::_W4_A = 1; + DataStorage::_W4_B = 0; + } else if (tmpMoveDirection==17){ + DataStorage::_W1_A = 0; + DataStorage::_W1_B = 1; + + DataStorage::_W2_A = 0; + DataStorage::_W2_B = 1; + + DataStorage::_W3_A = 0; + DataStorage::_W3_B = 1; + + DataStorage::_W4_A = 0; + DataStorage::_W4_B = 1; + } else if (tmpMoveDirection==40){ + DataStorage::_W1_A = 0; + DataStorage::_W1_B = 0; + + DataStorage::_W2_A = 0; + DataStorage::_W2_B = 0; + + DataStorage::_W3_A = 0; + DataStorage::_W3_B = 0; + + DataStorage::_W4_A = 0; + DataStorage::_W4_B = 0; + } +} + +void WM_pwmControl(DataStorage* tmpDS) { + printf("### WM pwmControl - Start\r\n"); + + /*PwmOut _W1_P(PD_14); + PwmOut _W2_P(PD_15); + PwmOut _W3_P(PE_12); + PwmOut _W4_P(PE_14);*/ + + //printf("### WM_pwmControl - 01\r\n"); + //printf("### WM_pwmControl - 01 - 01\r\n"); + //float tmpPeriod2 = (float)tmp[1]/1000000.0f; + //float tmpCycle2 = (float)tmp[2]/100.0f; + + /*printf("### WM_pwmControl - 01 - 01 - %d\r\n", (*tmpDS).get_testInt1()); + printf("### WM_pwmControl - 01 - 02 - %d\r\n", (*tmpDS).testInt2); + printf("### WM_pwmControl - 01 - 03 - %d\r\n", (*tmpDS).testInt3); + printf("### WM_pwmControl - 01 - 04 - %d\r\n", (*tmpDS).WM_PWM[1][0]); + printf("### WM_pwmControl - 01 - 05 - %d\r\n", (*tmpDS).WM_PWM[1][1]);*/ + + float *tmpPeriod2 = new float[4]; + float *tmpWrite2 = new float[4]; + //{(float)(*tmpDS).WM_PWM[1][0]/1000000.0f, 0.0f, 0.0f, 0.0f}; + tmpPeriod2[0] = (float)(*tmpDS).WM_PWM[1][0]/1000000.0f; + tmpPeriod2[1] = (float)(*tmpDS).WM_PWM[2][0]/1000000.0f; + tmpPeriod2[2] = (float)(*tmpDS).WM_PWM[3][0]/1000000.0f; + tmpPeriod2[3] = (float)(*tmpDS).WM_PWM[4][0]/1000000.0f; + //float tmpCycle2 = (float)(*tmpDS).WM_PWM[1][1]/100.0f; + //float tmpCycle3 = tmpCycle2+0.2f; + + //tmpPeriod2[1] = (float)(*tmpDS).WM_PWM[1][1]/100.0f; + //tmpPeriod2[2] = tmpPeriod2[1]+0.2f; + + tmpWrite2[0] = (float)(*tmpDS).WM_PWM[1][1]/100.0f; + tmpWrite2[1] = (float)(*tmpDS).WM_PWM[2][1]/100.0f; + tmpWrite2[2] = (float)(*tmpDS).WM_PWM[3][1]/100.0f; + tmpWrite2[3] = (float)(*tmpDS).WM_PWM[4][1]/100.0f; + + //if (tmpCycle3>1.0f) tmpCycle3=1.0f; + //if (tmpPeriod2[2]>1.0f) tmpPeriod2[2]=1.0f; + + //printf("### WM_pwmControl - 01 - %f\r\n", tmpCycle3); + printf("### WM pwmControl - 01 tmpPeriod2- %f; %f; %f; %f;\r\n", tmpPeriod2[0], tmpPeriod2[1], tmpPeriod2[2], tmpPeriod2[3]); + printf("### WM pwmControl - 02 tmpWrite2 - %f; %f; %f; %f;\r\n", tmpWrite2[0], tmpWrite2[1], tmpWrite2[2], tmpWrite2[3]); + + //printf("### WM_pwmControl - 02\r\n"); + + //PwmOut testPWM(PD_14); + //printf("### WM_pwmControl - 03\r\n"); + + //testPWM.period(tmpPeriod2); + //(*tmpPWM).period(tmpPeriod2); + + //(tmpDS->_WM_PWM_out01).period(tmpPeriod2[0]); + (tmpDS->_WM_PWM_out01).period(tmpPeriod2[0]); + (tmpDS->_WM_PWM_out02).period(tmpPeriod2[1]); + (tmpDS->_WM_PWM_out03).period(tmpPeriod2[2]); + (tmpDS->_WM_PWM_out04).period(tmpPeriod2[3]); + + //(*(*tmpDS).WM_PWM_out[0]).period(tmpPeriod2); + //printf("### WM_pwmControl - 04\r\n"); + + //testPWM.write(tmpCycle2); + //(*tmpPWM).write(tmpCycle2); + + //(tmpDS->_WM_PWM_out01).write(tmpCycle2); + //(tmpDS->_WM_PWM_out02).write(tmpCycle3); + + //(tmpDS->_WM_PWM_out01).write(tmpPeriod2[1]); + //(tmpDS->_WM_PWM_out02).write(tmpPeriod2[2]); + (tmpDS->_WM_PWM_out01).write(tmpWrite2[0]); + (tmpDS->_WM_PWM_out02).write(tmpWrite2[1]); + (tmpDS->_WM_PWM_out03).write(tmpWrite2[2]); + (tmpDS->_WM_PWM_out04).write(tmpWrite2[3]); + + //(*(*tmpDS).WM_PWM_out[0]).write(tmpCycle2); + //printf("### WM_pwmControl - 05\r\n"); + /*if (tmp[0]==1){ + PwmOut _W_P(PD_14); + _W_P.period(tmpPeriod2); + _W_P.write(tmpCycle2); + + } else if (tmp[0]==2){ + PwmOut _W_P(PD_15); + _W_P.period(tmpPeriod2); + _W_P.write(tmpCycle2); + + } else if (tmp[0]==3){ + PwmOut _W_P(PE_12); + _W_P.period(tmpPeriod2); + _W_P.write(tmpCycle2); + + } else if (tmp[0]==4){ + PwmOut _W_P(PE_14); + _W_P.period(tmpPeriod2); + _W_P.write(tmpCycle2); + }*/ + + delete [] tmpPeriod2; + delete [] tmpWrite2; + //delete tmpCycle2; + //delete tmpCycle3; + + printf("### WM pwmControl - End\r\n"); +} + +/*void WheelManager::runControl(int tmpMoveDirection, int tmpPeriod, int tmpCycle) { + moveDirectionControl(tmpMoveDirection); + //pwmControl(1, tmpPeriod, tmpCycle); + threadWM.start(callback(test, tmpMoveDirection)); + wait(1); +}*/ + +void WheelManager::runControl(DataStorage* tmpDS, int** tmp) { + printf("### WM runControl - Start\r\n"); + + /*PwmOut _W1_P(PD_14); + PwmOut _W2_P(PD_15); + PwmOut _W3_P(PE_12); + PwmOut _W4_P(PE_14);*/ + + //PwmOut* _W_P[4] = {&_W1_P, &_W2_P, &_W3_P, &_W4_P}; + + for (int ii=0;ii<5;ii++){ + for (int jj=0;jj<2;jj++){ + DataStorage::WM_PWM[ii][jj] = tmp[ii][jj]; + } + } + + + + //printf("### runControl - 01 - 01 - %d\r\n", DataStorage::testInt1); + //printf("### runControl - 01 - 02 - %d\r\n", DataStorage::testInt2); + //printf("### runControl - 01 - 03 - %d\r\n", DataStorage::testInt3); + //printf("### runControl - 01 - 01\r\n"); + + this->moveDirectionControl(tmp[0][0]); + + //pwmControl(tmp[0], tmp[1], tmp[2]); + //printf("### runControl - 01 - 02\r\n"); + + //PwmOut testPWM(PD_14); + //printf("### runControl - 02\r\n"); + //(*tmpThread).terminate(); + + //printf("### runControl - 03\r\n"); + //(*tmpThread).start(callback(WM_pwmControl, tmpDS)); + WM_pwmControl(tmpDS); + + this->moveDirectionControl(tmp[0][0]); + + //printf("### runControl - 04\r\n"); + //wait(1); + + //printf("### runControl - 05\r\n"); + //(*tmpThread).join(); + + printf("### WM runControl - End\r\n"); +} + +void WheelManager::runControl2(DataStorage* tmpDS, int** tmp) { + printf("### WM runControl2 - Start\r\n"); + + /*PwmOut _W1_P(PD_14); + PwmOut _W2_P(PD_15); + PwmOut _W3_P(PE_12); + PwmOut _W4_P(PE_14);*/ + + //PwmOut* _W_P[4] = {&_W1_P, &_W2_P, &_W3_P, &_W4_P}; + + for (int ii=0;ii<5;ii++){ + for (int jj=0;jj<2;jj++){ + DataStorage::WM_PWM[ii][jj] = tmp[ii][jj]; + } + } + + //printf("### runControl - 01 - 01 - %d\r\n", DataStorage::testInt1); + //printf("### runControl - 01 - 02 - %d\r\n", DataStorage::testInt2); + //printf("### runControl - 01 - 03 - %d\r\n", DataStorage::testInt3); + //printf("### runControl - 01 - 01\r\n"); + + this->moveDirectionControl(tmp[0][0]); + + //pwmControl(tmp[0], tmp[1], tmp[2]); + //printf("### runControl - 01 - 02\r\n"); + + //PwmOut testPWM(PD_14); + //printf("### runControl - 02\r\n"); + //(*tmpThread).terminate(); + + //printf("### runControl - 03\r\n"); + //(*tmpThread).start(callback(WM_pwmControl, tmpDS)); + WM_pwmControl(tmpDS); + + this->moveDirectionControl(tmp[0][0]); + + //printf("### runControl - 04\r\n"); + //wait(1); + + //printf("### runControl - 05\r\n"); + //(*tmpThread).join(); + + printf("### WM runControl2 - End\r\n"); +} + +void WheelManager::runControlStop(DataStorage* tmpDS) { + printf("### WM runControlStop - Start\r\n"); + + for (int ii=0;ii<5;ii++){ + DataStorage::WM_PWM[ii][0] = 1; + DataStorage::WM_PWM[ii][1] = 0; + } + + this->moveDirectionControl(40); + + WM_pwmControl(tmpDS); + + this->moveDirectionControl(40); + + printf("### WM runControlStop - End\r\n"); +} \ No newline at end of file