File content as of revision 0:61923a9dd110:

#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");
}