2018-12-21 3
Dependents: mbed-os-rest-api-V1-2 mbed-os-rest-api-V1-2-2
WheelManager.cpp
- Committer:
- asaewing
- Date:
- 2019-06-05
- Revision:
- 1:b794ac1a3aa6
- Parent:
- 0:61923a9dd110
File content as of revision 1:b794ac1a3aa6:
#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::set_PWM_status(1, 0, 0); DataStorage::set_PWM_status(2, 0, 0); DataStorage::set_PWM_status(3, 0, 0); DataStorage::set_PWM_status(4, 0, 0);*/ 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::set_PWM_status(1, 0, 0); DataStorage::set_PWM_status(2, 0, 0); DataStorage::set_PWM_status(3, 0, 0); DataStorage::set_PWM_status(4, 0, 0);*/ 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; //DataStorage::set_PWM_status(1, 0, 1); //DataStorage::set_PWM_status(4, 1, 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; //DataStorage::set_PWM_status(1, 1, 0); //DataStorage::set_PWM_status(4, 0, 1); } else if (tmpMoveDirection==2 || tmpMoveDirection==10){ DataStorage::_W1_A = 0; DataStorage::_W1_B = 0; DataStorage::_W4_A = 0; DataStorage::_W4_B = 0; //DataStorage::set_PWM_status(1, 0, 0); //DataStorage::set_PWM_status(4, 0, 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; //DataStorage::set_PWM_status(2, 0, 1); //DataStorage::set_PWM_status(3, 1, 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; //DataStorage::set_PWM_status(2, 1, 0); //DataStorage::set_PWM_status(3, 0, 1); } else if (tmpMoveDirection==6 || tmpMoveDirection==14){ DataStorage::_W2_A = 0; DataStorage::_W2_B = 0; DataStorage::_W3_A = 0; DataStorage::_W3_B = 0; //DataStorage::set_PWM_status(2, 0, 0); //DataStorage::set_PWM_status(3, 0, 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; /*DataStorage::set_PWM_status(1, 1, 0); DataStorage::set_PWM_status(2, 1, 0); DataStorage::set_PWM_status(3, 1, 0); DataStorage::set_PWM_status(4, 1, 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; /*DataStorage::set_PWM_status(1, 0, 1); DataStorage::set_PWM_status(2, 0, 1); DataStorage::set_PWM_status(3, 0, 1); DataStorage::set_PWM_status(4, 0, 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; /*DataStorage::set_PWM_status(1, 0, 0); DataStorage::set_PWM_status(2, 0, 0); DataStorage::set_PWM_status(3, 0, 0); DataStorage::set_PWM_status(4, 0, 0);*/ } } void WM_pwmControl(DataStorage* tmpDS) { printf("### WM pwmControl - Start\r\n"); float *tmpPeriod2 = new float[4]; float *tmpWrite2 = new float[4]; for (int ii=0;ii<4;ii++) { tmpPeriod2[ii] = (float)(*tmpDS).WM_PWM[ii+1][0]/1000000.0f; tmpWrite2[ii] = (float)(*tmpDS).WM_PWM[ii+1][1]/100.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; 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;*/ printf("### WM pwmControl - 01 tmpPeriod2- %f; %f; %f; %f;\r\n", tmpPeriod2[0], tmpPeriod2[1], tmpPeriod2[2], tmpPeriod2[3]); //for (int ii=0;ii<4;ii++) tmpDS->set_PWM_period(ii, tmpPeriod2[ii]); (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]); printf("### WM pwmControl - 02 tmpWrite2 - %f; %f; %f; %f;\r\n", tmpWrite2[0], tmpWrite2[1], tmpWrite2[2], tmpWrite2[3]); //for (int ii=0;ii<4;ii++) tmpDS->set_PWM_write(ii, tmpWrite2[ii]); (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]); delete [] tmpPeriod2; delete [] tmpWrite2; 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"); for (int ii=0;ii<5;ii++){ for (int jj=0;jj<2;jj++){ DataStorage::WM_PWM[ii][jj] = tmp[ii][jj]; } } this->moveDirectionControl(tmp[0][0]); WM_pwmControl(tmpDS); this->moveDirectionControl(tmp[0][0]); 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"); }