2018-12-21 3
Dependents: mbed-os-rest-api-V1-2 mbed-os-rest-api-V1-2-2
WheelManager.cpp@0:61923a9dd110, 2018-12-21 (annotated)
- Committer:
- asaewing
- Date:
- Fri Dec 21 07:54:10 2018 +0000
- Revision:
- 0:61923a9dd110
- Child:
- 1:b794ac1a3aa6
First 2018-12-21
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
asaewing | 0:61923a9dd110 | 1 | #include "module_for_all.h" |
asaewing | 0:61923a9dd110 | 2 | |
asaewing | 0:61923a9dd110 | 3 | /*DigitalOut _W1_A(PC_6); |
asaewing | 0:61923a9dd110 | 4 | DigitalOut _W1_B(PD_8); |
asaewing | 0:61923a9dd110 | 5 | |
asaewing | 0:61923a9dd110 | 6 | DigitalOut _W2_A(PA_12); |
asaewing | 0:61923a9dd110 | 7 | DigitalOut _W2_B(PA_11); |
asaewing | 0:61923a9dd110 | 8 | |
asaewing | 0:61923a9dd110 | 9 | DigitalOut _W3_A(PB_12); |
asaewing | 0:61923a9dd110 | 10 | DigitalOut _W3_B(PB_11); |
asaewing | 0:61923a9dd110 | 11 | |
asaewing | 0:61923a9dd110 | 12 | DigitalOut _W4_A(PB_2); |
asaewing | 0:61923a9dd110 | 13 | DigitalOut _W4_B(PB_15);*/ |
asaewing | 0:61923a9dd110 | 14 | |
asaewing | 0:61923a9dd110 | 15 | |
asaewing | 0:61923a9dd110 | 16 | WheelManager::WheelManager() { |
asaewing | 0:61923a9dd110 | 17 | DataStorage::_W1_A = 0; |
asaewing | 0:61923a9dd110 | 18 | DataStorage::_W1_B = 0; |
asaewing | 0:61923a9dd110 | 19 | |
asaewing | 0:61923a9dd110 | 20 | DataStorage::_W2_A = 0; |
asaewing | 0:61923a9dd110 | 21 | DataStorage::_W2_B = 0; |
asaewing | 0:61923a9dd110 | 22 | |
asaewing | 0:61923a9dd110 | 23 | DataStorage::_W3_A = 0; |
asaewing | 0:61923a9dd110 | 24 | DataStorage::_W3_B = 0; |
asaewing | 0:61923a9dd110 | 25 | |
asaewing | 0:61923a9dd110 | 26 | DataStorage::_W4_A = 0; |
asaewing | 0:61923a9dd110 | 27 | DataStorage::_W4_B = 0; |
asaewing | 0:61923a9dd110 | 28 | } |
asaewing | 0:61923a9dd110 | 29 | |
asaewing | 0:61923a9dd110 | 30 | WheelManager::WheelManager(std::string tmpTest) { |
asaewing | 0:61923a9dd110 | 31 | DataStorage::_W1_A = 0; |
asaewing | 0:61923a9dd110 | 32 | DataStorage::_W1_B = 0; |
asaewing | 0:61923a9dd110 | 33 | |
asaewing | 0:61923a9dd110 | 34 | DataStorage::_W2_A = 0; |
asaewing | 0:61923a9dd110 | 35 | DataStorage::_W2_B = 0; |
asaewing | 0:61923a9dd110 | 36 | |
asaewing | 0:61923a9dd110 | 37 | DataStorage::_W3_A = 0; |
asaewing | 0:61923a9dd110 | 38 | DataStorage::_W3_B = 0; |
asaewing | 0:61923a9dd110 | 39 | |
asaewing | 0:61923a9dd110 | 40 | DataStorage::_W4_A = 0; |
asaewing | 0:61923a9dd110 | 41 | DataStorage::_W4_B = 0; |
asaewing | 0:61923a9dd110 | 42 | } |
asaewing | 0:61923a9dd110 | 43 | |
asaewing | 0:61923a9dd110 | 44 | void WheelManager::moveDirectionControl(int tmpMoveDirection) { |
asaewing | 0:61923a9dd110 | 45 | _moveDirection = tmpMoveDirection; |
asaewing | 0:61923a9dd110 | 46 | |
asaewing | 0:61923a9dd110 | 47 | //W1 W4// |
asaewing | 0:61923a9dd110 | 48 | if (tmpMoveDirection>=5 && tmpMoveDirection<=9 || tmpMoveDirection==3 || tmpMoveDirection==12){ |
asaewing | 0:61923a9dd110 | 49 | DataStorage::_W1_A = 0; |
asaewing | 0:61923a9dd110 | 50 | DataStorage::_W1_B = 1; |
asaewing | 0:61923a9dd110 | 51 | |
asaewing | 0:61923a9dd110 | 52 | DataStorage::_W4_A = 1; |
asaewing | 0:61923a9dd110 | 53 | DataStorage::_W4_B = 0; |
asaewing | 0:61923a9dd110 | 54 | } else if (tmpMoveDirection>=13 && tmpMoveDirection<=15 || tmpMoveDirection==0 || tmpMoveDirection==1 || tmpMoveDirection==4 || tmpMoveDirection==11){ |
asaewing | 0:61923a9dd110 | 55 | DataStorage::_W1_A = 1; |
asaewing | 0:61923a9dd110 | 56 | DataStorage::_W1_B = 0; |
asaewing | 0:61923a9dd110 | 57 | |
asaewing | 0:61923a9dd110 | 58 | DataStorage::_W4_A = 0; |
asaewing | 0:61923a9dd110 | 59 | DataStorage::_W4_B = 1; |
asaewing | 0:61923a9dd110 | 60 | } else if (tmpMoveDirection==2 || tmpMoveDirection==10){ |
asaewing | 0:61923a9dd110 | 61 | DataStorage::_W1_A = 0; |
asaewing | 0:61923a9dd110 | 62 | DataStorage::_W1_B = 0; |
asaewing | 0:61923a9dd110 | 63 | |
asaewing | 0:61923a9dd110 | 64 | DataStorage::_W4_A = 0; |
asaewing | 0:61923a9dd110 | 65 | DataStorage::_W4_B = 0; |
asaewing | 0:61923a9dd110 | 66 | } |
asaewing | 0:61923a9dd110 | 67 | |
asaewing | 0:61923a9dd110 | 68 | //W2 W3// |
asaewing | 0:61923a9dd110 | 69 | if (tmpMoveDirection>=0 && tmpMoveDirection<=3 || tmpMoveDirection==5 || tmpMoveDirection==12 || tmpMoveDirection==15){ |
asaewing | 0:61923a9dd110 | 70 | DataStorage::_W2_A = 0; |
asaewing | 0:61923a9dd110 | 71 | DataStorage::_W2_B = 1; |
asaewing | 0:61923a9dd110 | 72 | |
asaewing | 0:61923a9dd110 | 73 | DataStorage::_W3_A = 1; |
asaewing | 0:61923a9dd110 | 74 | DataStorage::_W3_B = 0; |
asaewing | 0:61923a9dd110 | 75 | } else if (tmpMoveDirection>=7 && tmpMoveDirection<=11 || tmpMoveDirection==4 || tmpMoveDirection==13){ |
asaewing | 0:61923a9dd110 | 76 | DataStorage::_W2_A = 1; |
asaewing | 0:61923a9dd110 | 77 | DataStorage::_W2_B = 0; |
asaewing | 0:61923a9dd110 | 78 | |
asaewing | 0:61923a9dd110 | 79 | DataStorage::_W3_A = 0; |
asaewing | 0:61923a9dd110 | 80 | DataStorage::_W3_B = 1; |
asaewing | 0:61923a9dd110 | 81 | } else if (tmpMoveDirection==6 || tmpMoveDirection==14){ |
asaewing | 0:61923a9dd110 | 82 | DataStorage::_W2_A = 0; |
asaewing | 0:61923a9dd110 | 83 | DataStorage::_W2_B = 0; |
asaewing | 0:61923a9dd110 | 84 | |
asaewing | 0:61923a9dd110 | 85 | DataStorage::_W3_A = 0; |
asaewing | 0:61923a9dd110 | 86 | DataStorage::_W3_B = 0; |
asaewing | 0:61923a9dd110 | 87 | } |
asaewing | 0:61923a9dd110 | 88 | |
asaewing | 0:61923a9dd110 | 89 | //W1 W2 W3 W4// |
asaewing | 0:61923a9dd110 | 90 | if (tmpMoveDirection==16){ |
asaewing | 0:61923a9dd110 | 91 | DataStorage::_W1_A = 1; |
asaewing | 0:61923a9dd110 | 92 | DataStorage::_W1_B = 0; |
asaewing | 0:61923a9dd110 | 93 | |
asaewing | 0:61923a9dd110 | 94 | DataStorage::_W2_A = 1; |
asaewing | 0:61923a9dd110 | 95 | DataStorage::_W2_B = 0; |
asaewing | 0:61923a9dd110 | 96 | |
asaewing | 0:61923a9dd110 | 97 | DataStorage::_W3_A = 1; |
asaewing | 0:61923a9dd110 | 98 | DataStorage::_W3_B = 0; |
asaewing | 0:61923a9dd110 | 99 | |
asaewing | 0:61923a9dd110 | 100 | DataStorage::_W4_A = 1; |
asaewing | 0:61923a9dd110 | 101 | DataStorage::_W4_B = 0; |
asaewing | 0:61923a9dd110 | 102 | } else if (tmpMoveDirection==17){ |
asaewing | 0:61923a9dd110 | 103 | DataStorage::_W1_A = 0; |
asaewing | 0:61923a9dd110 | 104 | DataStorage::_W1_B = 1; |
asaewing | 0:61923a9dd110 | 105 | |
asaewing | 0:61923a9dd110 | 106 | DataStorage::_W2_A = 0; |
asaewing | 0:61923a9dd110 | 107 | DataStorage::_W2_B = 1; |
asaewing | 0:61923a9dd110 | 108 | |
asaewing | 0:61923a9dd110 | 109 | DataStorage::_W3_A = 0; |
asaewing | 0:61923a9dd110 | 110 | DataStorage::_W3_B = 1; |
asaewing | 0:61923a9dd110 | 111 | |
asaewing | 0:61923a9dd110 | 112 | DataStorage::_W4_A = 0; |
asaewing | 0:61923a9dd110 | 113 | DataStorage::_W4_B = 1; |
asaewing | 0:61923a9dd110 | 114 | } else if (tmpMoveDirection==40){ |
asaewing | 0:61923a9dd110 | 115 | DataStorage::_W1_A = 0; |
asaewing | 0:61923a9dd110 | 116 | DataStorage::_W1_B = 0; |
asaewing | 0:61923a9dd110 | 117 | |
asaewing | 0:61923a9dd110 | 118 | DataStorage::_W2_A = 0; |
asaewing | 0:61923a9dd110 | 119 | DataStorage::_W2_B = 0; |
asaewing | 0:61923a9dd110 | 120 | |
asaewing | 0:61923a9dd110 | 121 | DataStorage::_W3_A = 0; |
asaewing | 0:61923a9dd110 | 122 | DataStorage::_W3_B = 0; |
asaewing | 0:61923a9dd110 | 123 | |
asaewing | 0:61923a9dd110 | 124 | DataStorage::_W4_A = 0; |
asaewing | 0:61923a9dd110 | 125 | DataStorage::_W4_B = 0; |
asaewing | 0:61923a9dd110 | 126 | } |
asaewing | 0:61923a9dd110 | 127 | } |
asaewing | 0:61923a9dd110 | 128 | |
asaewing | 0:61923a9dd110 | 129 | void WM_pwmControl(DataStorage* tmpDS) { |
asaewing | 0:61923a9dd110 | 130 | printf("### WM pwmControl - Start\r\n"); |
asaewing | 0:61923a9dd110 | 131 | |
asaewing | 0:61923a9dd110 | 132 | /*PwmOut _W1_P(PD_14); |
asaewing | 0:61923a9dd110 | 133 | PwmOut _W2_P(PD_15); |
asaewing | 0:61923a9dd110 | 134 | PwmOut _W3_P(PE_12); |
asaewing | 0:61923a9dd110 | 135 | PwmOut _W4_P(PE_14);*/ |
asaewing | 0:61923a9dd110 | 136 | |
asaewing | 0:61923a9dd110 | 137 | //printf("### WM_pwmControl - 01\r\n"); |
asaewing | 0:61923a9dd110 | 138 | //printf("### WM_pwmControl - 01 - 01\r\n"); |
asaewing | 0:61923a9dd110 | 139 | //float tmpPeriod2 = (float)tmp[1]/1000000.0f; |
asaewing | 0:61923a9dd110 | 140 | //float tmpCycle2 = (float)tmp[2]/100.0f; |
asaewing | 0:61923a9dd110 | 141 | |
asaewing | 0:61923a9dd110 | 142 | /*printf("### WM_pwmControl - 01 - 01 - %d\r\n", (*tmpDS).get_testInt1()); |
asaewing | 0:61923a9dd110 | 143 | printf("### WM_pwmControl - 01 - 02 - %d\r\n", (*tmpDS).testInt2); |
asaewing | 0:61923a9dd110 | 144 | printf("### WM_pwmControl - 01 - 03 - %d\r\n", (*tmpDS).testInt3); |
asaewing | 0:61923a9dd110 | 145 | printf("### WM_pwmControl - 01 - 04 - %d\r\n", (*tmpDS).WM_PWM[1][0]); |
asaewing | 0:61923a9dd110 | 146 | printf("### WM_pwmControl - 01 - 05 - %d\r\n", (*tmpDS).WM_PWM[1][1]);*/ |
asaewing | 0:61923a9dd110 | 147 | |
asaewing | 0:61923a9dd110 | 148 | float *tmpPeriod2 = new float[4]; |
asaewing | 0:61923a9dd110 | 149 | float *tmpWrite2 = new float[4]; |
asaewing | 0:61923a9dd110 | 150 | //{(float)(*tmpDS).WM_PWM[1][0]/1000000.0f, 0.0f, 0.0f, 0.0f}; |
asaewing | 0:61923a9dd110 | 151 | tmpPeriod2[0] = (float)(*tmpDS).WM_PWM[1][0]/1000000.0f; |
asaewing | 0:61923a9dd110 | 152 | tmpPeriod2[1] = (float)(*tmpDS).WM_PWM[2][0]/1000000.0f; |
asaewing | 0:61923a9dd110 | 153 | tmpPeriod2[2] = (float)(*tmpDS).WM_PWM[3][0]/1000000.0f; |
asaewing | 0:61923a9dd110 | 154 | tmpPeriod2[3] = (float)(*tmpDS).WM_PWM[4][0]/1000000.0f; |
asaewing | 0:61923a9dd110 | 155 | //float tmpCycle2 = (float)(*tmpDS).WM_PWM[1][1]/100.0f; |
asaewing | 0:61923a9dd110 | 156 | //float tmpCycle3 = tmpCycle2+0.2f; |
asaewing | 0:61923a9dd110 | 157 | |
asaewing | 0:61923a9dd110 | 158 | //tmpPeriod2[1] = (float)(*tmpDS).WM_PWM[1][1]/100.0f; |
asaewing | 0:61923a9dd110 | 159 | //tmpPeriod2[2] = tmpPeriod2[1]+0.2f; |
asaewing | 0:61923a9dd110 | 160 | |
asaewing | 0:61923a9dd110 | 161 | tmpWrite2[0] = (float)(*tmpDS).WM_PWM[1][1]/100.0f; |
asaewing | 0:61923a9dd110 | 162 | tmpWrite2[1] = (float)(*tmpDS).WM_PWM[2][1]/100.0f; |
asaewing | 0:61923a9dd110 | 163 | tmpWrite2[2] = (float)(*tmpDS).WM_PWM[3][1]/100.0f; |
asaewing | 0:61923a9dd110 | 164 | tmpWrite2[3] = (float)(*tmpDS).WM_PWM[4][1]/100.0f; |
asaewing | 0:61923a9dd110 | 165 | |
asaewing | 0:61923a9dd110 | 166 | //if (tmpCycle3>1.0f) tmpCycle3=1.0f; |
asaewing | 0:61923a9dd110 | 167 | //if (tmpPeriod2[2]>1.0f) tmpPeriod2[2]=1.0f; |
asaewing | 0:61923a9dd110 | 168 | |
asaewing | 0:61923a9dd110 | 169 | //printf("### WM_pwmControl - 01 - %f\r\n", tmpCycle3); |
asaewing | 0:61923a9dd110 | 170 | printf("### WM pwmControl - 01 tmpPeriod2- %f; %f; %f; %f;\r\n", tmpPeriod2[0], tmpPeriod2[1], tmpPeriod2[2], tmpPeriod2[3]); |
asaewing | 0:61923a9dd110 | 171 | printf("### WM pwmControl - 02 tmpWrite2 - %f; %f; %f; %f;\r\n", tmpWrite2[0], tmpWrite2[1], tmpWrite2[2], tmpWrite2[3]); |
asaewing | 0:61923a9dd110 | 172 | |
asaewing | 0:61923a9dd110 | 173 | //printf("### WM_pwmControl - 02\r\n"); |
asaewing | 0:61923a9dd110 | 174 | |
asaewing | 0:61923a9dd110 | 175 | //PwmOut testPWM(PD_14); |
asaewing | 0:61923a9dd110 | 176 | //printf("### WM_pwmControl - 03\r\n"); |
asaewing | 0:61923a9dd110 | 177 | |
asaewing | 0:61923a9dd110 | 178 | //testPWM.period(tmpPeriod2); |
asaewing | 0:61923a9dd110 | 179 | //(*tmpPWM).period(tmpPeriod2); |
asaewing | 0:61923a9dd110 | 180 | |
asaewing | 0:61923a9dd110 | 181 | //(tmpDS->_WM_PWM_out01).period(tmpPeriod2[0]); |
asaewing | 0:61923a9dd110 | 182 | (tmpDS->_WM_PWM_out01).period(tmpPeriod2[0]); |
asaewing | 0:61923a9dd110 | 183 | (tmpDS->_WM_PWM_out02).period(tmpPeriod2[1]); |
asaewing | 0:61923a9dd110 | 184 | (tmpDS->_WM_PWM_out03).period(tmpPeriod2[2]); |
asaewing | 0:61923a9dd110 | 185 | (tmpDS->_WM_PWM_out04).period(tmpPeriod2[3]); |
asaewing | 0:61923a9dd110 | 186 | |
asaewing | 0:61923a9dd110 | 187 | //(*(*tmpDS).WM_PWM_out[0]).period(tmpPeriod2); |
asaewing | 0:61923a9dd110 | 188 | //printf("### WM_pwmControl - 04\r\n"); |
asaewing | 0:61923a9dd110 | 189 | |
asaewing | 0:61923a9dd110 | 190 | //testPWM.write(tmpCycle2); |
asaewing | 0:61923a9dd110 | 191 | //(*tmpPWM).write(tmpCycle2); |
asaewing | 0:61923a9dd110 | 192 | |
asaewing | 0:61923a9dd110 | 193 | //(tmpDS->_WM_PWM_out01).write(tmpCycle2); |
asaewing | 0:61923a9dd110 | 194 | //(tmpDS->_WM_PWM_out02).write(tmpCycle3); |
asaewing | 0:61923a9dd110 | 195 | |
asaewing | 0:61923a9dd110 | 196 | //(tmpDS->_WM_PWM_out01).write(tmpPeriod2[1]); |
asaewing | 0:61923a9dd110 | 197 | //(tmpDS->_WM_PWM_out02).write(tmpPeriod2[2]); |
asaewing | 0:61923a9dd110 | 198 | (tmpDS->_WM_PWM_out01).write(tmpWrite2[0]); |
asaewing | 0:61923a9dd110 | 199 | (tmpDS->_WM_PWM_out02).write(tmpWrite2[1]); |
asaewing | 0:61923a9dd110 | 200 | (tmpDS->_WM_PWM_out03).write(tmpWrite2[2]); |
asaewing | 0:61923a9dd110 | 201 | (tmpDS->_WM_PWM_out04).write(tmpWrite2[3]); |
asaewing | 0:61923a9dd110 | 202 | |
asaewing | 0:61923a9dd110 | 203 | //(*(*tmpDS).WM_PWM_out[0]).write(tmpCycle2); |
asaewing | 0:61923a9dd110 | 204 | //printf("### WM_pwmControl - 05\r\n"); |
asaewing | 0:61923a9dd110 | 205 | /*if (tmp[0]==1){ |
asaewing | 0:61923a9dd110 | 206 | PwmOut _W_P(PD_14); |
asaewing | 0:61923a9dd110 | 207 | _W_P.period(tmpPeriod2); |
asaewing | 0:61923a9dd110 | 208 | _W_P.write(tmpCycle2); |
asaewing | 0:61923a9dd110 | 209 | |
asaewing | 0:61923a9dd110 | 210 | } else if (tmp[0]==2){ |
asaewing | 0:61923a9dd110 | 211 | PwmOut _W_P(PD_15); |
asaewing | 0:61923a9dd110 | 212 | _W_P.period(tmpPeriod2); |
asaewing | 0:61923a9dd110 | 213 | _W_P.write(tmpCycle2); |
asaewing | 0:61923a9dd110 | 214 | |
asaewing | 0:61923a9dd110 | 215 | } else if (tmp[0]==3){ |
asaewing | 0:61923a9dd110 | 216 | PwmOut _W_P(PE_12); |
asaewing | 0:61923a9dd110 | 217 | _W_P.period(tmpPeriod2); |
asaewing | 0:61923a9dd110 | 218 | _W_P.write(tmpCycle2); |
asaewing | 0:61923a9dd110 | 219 | |
asaewing | 0:61923a9dd110 | 220 | } else if (tmp[0]==4){ |
asaewing | 0:61923a9dd110 | 221 | PwmOut _W_P(PE_14); |
asaewing | 0:61923a9dd110 | 222 | _W_P.period(tmpPeriod2); |
asaewing | 0:61923a9dd110 | 223 | _W_P.write(tmpCycle2); |
asaewing | 0:61923a9dd110 | 224 | }*/ |
asaewing | 0:61923a9dd110 | 225 | |
asaewing | 0:61923a9dd110 | 226 | delete [] tmpPeriod2; |
asaewing | 0:61923a9dd110 | 227 | delete [] tmpWrite2; |
asaewing | 0:61923a9dd110 | 228 | //delete tmpCycle2; |
asaewing | 0:61923a9dd110 | 229 | //delete tmpCycle3; |
asaewing | 0:61923a9dd110 | 230 | |
asaewing | 0:61923a9dd110 | 231 | printf("### WM pwmControl - End\r\n"); |
asaewing | 0:61923a9dd110 | 232 | } |
asaewing | 0:61923a9dd110 | 233 | |
asaewing | 0:61923a9dd110 | 234 | /*void WheelManager::runControl(int tmpMoveDirection, int tmpPeriod, int tmpCycle) { |
asaewing | 0:61923a9dd110 | 235 | moveDirectionControl(tmpMoveDirection); |
asaewing | 0:61923a9dd110 | 236 | //pwmControl(1, tmpPeriod, tmpCycle); |
asaewing | 0:61923a9dd110 | 237 | threadWM.start(callback(test, tmpMoveDirection)); |
asaewing | 0:61923a9dd110 | 238 | wait(1); |
asaewing | 0:61923a9dd110 | 239 | }*/ |
asaewing | 0:61923a9dd110 | 240 | |
asaewing | 0:61923a9dd110 | 241 | void WheelManager::runControl(DataStorage* tmpDS, int** tmp) { |
asaewing | 0:61923a9dd110 | 242 | printf("### WM runControl - Start\r\n"); |
asaewing | 0:61923a9dd110 | 243 | |
asaewing | 0:61923a9dd110 | 244 | /*PwmOut _W1_P(PD_14); |
asaewing | 0:61923a9dd110 | 245 | PwmOut _W2_P(PD_15); |
asaewing | 0:61923a9dd110 | 246 | PwmOut _W3_P(PE_12); |
asaewing | 0:61923a9dd110 | 247 | PwmOut _W4_P(PE_14);*/ |
asaewing | 0:61923a9dd110 | 248 | |
asaewing | 0:61923a9dd110 | 249 | //PwmOut* _W_P[4] = {&_W1_P, &_W2_P, &_W3_P, &_W4_P}; |
asaewing | 0:61923a9dd110 | 250 | |
asaewing | 0:61923a9dd110 | 251 | for (int ii=0;ii<5;ii++){ |
asaewing | 0:61923a9dd110 | 252 | for (int jj=0;jj<2;jj++){ |
asaewing | 0:61923a9dd110 | 253 | DataStorage::WM_PWM[ii][jj] = tmp[ii][jj]; |
asaewing | 0:61923a9dd110 | 254 | } |
asaewing | 0:61923a9dd110 | 255 | } |
asaewing | 0:61923a9dd110 | 256 | |
asaewing | 0:61923a9dd110 | 257 | |
asaewing | 0:61923a9dd110 | 258 | |
asaewing | 0:61923a9dd110 | 259 | //printf("### runControl - 01 - 01 - %d\r\n", DataStorage::testInt1); |
asaewing | 0:61923a9dd110 | 260 | //printf("### runControl - 01 - 02 - %d\r\n", DataStorage::testInt2); |
asaewing | 0:61923a9dd110 | 261 | //printf("### runControl - 01 - 03 - %d\r\n", DataStorage::testInt3); |
asaewing | 0:61923a9dd110 | 262 | //printf("### runControl - 01 - 01\r\n"); |
asaewing | 0:61923a9dd110 | 263 | |
asaewing | 0:61923a9dd110 | 264 | this->moveDirectionControl(tmp[0][0]); |
asaewing | 0:61923a9dd110 | 265 | |
asaewing | 0:61923a9dd110 | 266 | //pwmControl(tmp[0], tmp[1], tmp[2]); |
asaewing | 0:61923a9dd110 | 267 | //printf("### runControl - 01 - 02\r\n"); |
asaewing | 0:61923a9dd110 | 268 | |
asaewing | 0:61923a9dd110 | 269 | //PwmOut testPWM(PD_14); |
asaewing | 0:61923a9dd110 | 270 | //printf("### runControl - 02\r\n"); |
asaewing | 0:61923a9dd110 | 271 | //(*tmpThread).terminate(); |
asaewing | 0:61923a9dd110 | 272 | |
asaewing | 0:61923a9dd110 | 273 | //printf("### runControl - 03\r\n"); |
asaewing | 0:61923a9dd110 | 274 | //(*tmpThread).start(callback(WM_pwmControl, tmpDS)); |
asaewing | 0:61923a9dd110 | 275 | WM_pwmControl(tmpDS); |
asaewing | 0:61923a9dd110 | 276 | |
asaewing | 0:61923a9dd110 | 277 | this->moveDirectionControl(tmp[0][0]); |
asaewing | 0:61923a9dd110 | 278 | |
asaewing | 0:61923a9dd110 | 279 | //printf("### runControl - 04\r\n"); |
asaewing | 0:61923a9dd110 | 280 | //wait(1); |
asaewing | 0:61923a9dd110 | 281 | |
asaewing | 0:61923a9dd110 | 282 | //printf("### runControl - 05\r\n"); |
asaewing | 0:61923a9dd110 | 283 | //(*tmpThread).join(); |
asaewing | 0:61923a9dd110 | 284 | |
asaewing | 0:61923a9dd110 | 285 | printf("### WM runControl - End\r\n"); |
asaewing | 0:61923a9dd110 | 286 | } |
asaewing | 0:61923a9dd110 | 287 | |
asaewing | 0:61923a9dd110 | 288 | void WheelManager::runControl2(DataStorage* tmpDS, int** tmp) { |
asaewing | 0:61923a9dd110 | 289 | printf("### WM runControl2 - Start\r\n"); |
asaewing | 0:61923a9dd110 | 290 | |
asaewing | 0:61923a9dd110 | 291 | /*PwmOut _W1_P(PD_14); |
asaewing | 0:61923a9dd110 | 292 | PwmOut _W2_P(PD_15); |
asaewing | 0:61923a9dd110 | 293 | PwmOut _W3_P(PE_12); |
asaewing | 0:61923a9dd110 | 294 | PwmOut _W4_P(PE_14);*/ |
asaewing | 0:61923a9dd110 | 295 | |
asaewing | 0:61923a9dd110 | 296 | //PwmOut* _W_P[4] = {&_W1_P, &_W2_P, &_W3_P, &_W4_P}; |
asaewing | 0:61923a9dd110 | 297 | |
asaewing | 0:61923a9dd110 | 298 | for (int ii=0;ii<5;ii++){ |
asaewing | 0:61923a9dd110 | 299 | for (int jj=0;jj<2;jj++){ |
asaewing | 0:61923a9dd110 | 300 | DataStorage::WM_PWM[ii][jj] = tmp[ii][jj]; |
asaewing | 0:61923a9dd110 | 301 | } |
asaewing | 0:61923a9dd110 | 302 | } |
asaewing | 0:61923a9dd110 | 303 | |
asaewing | 0:61923a9dd110 | 304 | //printf("### runControl - 01 - 01 - %d\r\n", DataStorage::testInt1); |
asaewing | 0:61923a9dd110 | 305 | //printf("### runControl - 01 - 02 - %d\r\n", DataStorage::testInt2); |
asaewing | 0:61923a9dd110 | 306 | //printf("### runControl - 01 - 03 - %d\r\n", DataStorage::testInt3); |
asaewing | 0:61923a9dd110 | 307 | //printf("### runControl - 01 - 01\r\n"); |
asaewing | 0:61923a9dd110 | 308 | |
asaewing | 0:61923a9dd110 | 309 | this->moveDirectionControl(tmp[0][0]); |
asaewing | 0:61923a9dd110 | 310 | |
asaewing | 0:61923a9dd110 | 311 | //pwmControl(tmp[0], tmp[1], tmp[2]); |
asaewing | 0:61923a9dd110 | 312 | //printf("### runControl - 01 - 02\r\n"); |
asaewing | 0:61923a9dd110 | 313 | |
asaewing | 0:61923a9dd110 | 314 | //PwmOut testPWM(PD_14); |
asaewing | 0:61923a9dd110 | 315 | //printf("### runControl - 02\r\n"); |
asaewing | 0:61923a9dd110 | 316 | //(*tmpThread).terminate(); |
asaewing | 0:61923a9dd110 | 317 | |
asaewing | 0:61923a9dd110 | 318 | //printf("### runControl - 03\r\n"); |
asaewing | 0:61923a9dd110 | 319 | //(*tmpThread).start(callback(WM_pwmControl, tmpDS)); |
asaewing | 0:61923a9dd110 | 320 | WM_pwmControl(tmpDS); |
asaewing | 0:61923a9dd110 | 321 | |
asaewing | 0:61923a9dd110 | 322 | this->moveDirectionControl(tmp[0][0]); |
asaewing | 0:61923a9dd110 | 323 | |
asaewing | 0:61923a9dd110 | 324 | //printf("### runControl - 04\r\n"); |
asaewing | 0:61923a9dd110 | 325 | //wait(1); |
asaewing | 0:61923a9dd110 | 326 | |
asaewing | 0:61923a9dd110 | 327 | //printf("### runControl - 05\r\n"); |
asaewing | 0:61923a9dd110 | 328 | //(*tmpThread).join(); |
asaewing | 0:61923a9dd110 | 329 | |
asaewing | 0:61923a9dd110 | 330 | printf("### WM runControl2 - End\r\n"); |
asaewing | 0:61923a9dd110 | 331 | } |
asaewing | 0:61923a9dd110 | 332 | |
asaewing | 0:61923a9dd110 | 333 | void WheelManager::runControlStop(DataStorage* tmpDS) { |
asaewing | 0:61923a9dd110 | 334 | printf("### WM runControlStop - Start\r\n"); |
asaewing | 0:61923a9dd110 | 335 | |
asaewing | 0:61923a9dd110 | 336 | for (int ii=0;ii<5;ii++){ |
asaewing | 0:61923a9dd110 | 337 | DataStorage::WM_PWM[ii][0] = 1; |
asaewing | 0:61923a9dd110 | 338 | DataStorage::WM_PWM[ii][1] = 0; |
asaewing | 0:61923a9dd110 | 339 | } |
asaewing | 0:61923a9dd110 | 340 | |
asaewing | 0:61923a9dd110 | 341 | this->moveDirectionControl(40); |
asaewing | 0:61923a9dd110 | 342 | |
asaewing | 0:61923a9dd110 | 343 | WM_pwmControl(tmpDS); |
asaewing | 0:61923a9dd110 | 344 | |
asaewing | 0:61923a9dd110 | 345 | this->moveDirectionControl(40); |
asaewing | 0:61923a9dd110 | 346 | |
asaewing | 0:61923a9dd110 | 347 | printf("### WM runControlStop - End\r\n"); |
asaewing | 0:61923a9dd110 | 348 | } |