Asa Ewing / WheelManager

2018-12-21 3

Dependents:   mbed-os-rest-api-V1-2 mbed-os-rest-api-V1-2-2

WheelManager.cpp@1:b794ac1a3aa6, 2019-06-05 (annotated)

Committer:
asaewing
Date:
Wed Jun 05 07:54:02 2019 +0000
Revision:
1:b794ac1a3aa6
Parent:
0:61923a9dd110 
1.0

Who changed what in which revision?

UserRevisionLine numberNew 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 1:b794ac1a3aa6 17 /*DataStorage::set_PWM_status(1, 0, 0);
asaewing 1:b794ac1a3aa6 18 DataStorage::set_PWM_status(2, 0, 0);
asaewing 1:b794ac1a3aa6 19 DataStorage::set_PWM_status(3, 0, 0);
asaewing 1:b794ac1a3aa6 20 DataStorage::set_PWM_status(4, 0, 0);*/
asaewing 0:61923a9dd110 21 DataStorage::_W1_A = 0;
asaewing 0:61923a9dd110 22 DataStorage::_W1_B = 0;
asaewing 0:61923a9dd110 23
asaewing 0:61923a9dd110 24 DataStorage::_W2_A = 0;
asaewing 0:61923a9dd110 25 DataStorage::_W2_B = 0;
asaewing 0:61923a9dd110 26
asaewing 0:61923a9dd110 27 DataStorage::_W3_A = 0;
asaewing 0:61923a9dd110 28 DataStorage::_W3_B = 0;
asaewing 0:61923a9dd110 29
asaewing 0:61923a9dd110 30 DataStorage::_W4_A = 0;
asaewing 0:61923a9dd110 31 DataStorage::_W4_B = 0;
asaewing 0:61923a9dd110 32 }
asaewing 0:61923a9dd110 33
asaewing 0:61923a9dd110 34 WheelManager::WheelManager(std::string tmpTest) {
asaewing 1:b794ac1a3aa6 35 /*DataStorage::set_PWM_status(1, 0, 0);
asaewing 1:b794ac1a3aa6 36 DataStorage::set_PWM_status(2, 0, 0);
asaewing 1:b794ac1a3aa6 37 DataStorage::set_PWM_status(3, 0, 0);
asaewing 1:b794ac1a3aa6 38 DataStorage::set_PWM_status(4, 0, 0);*/
asaewing 1:b794ac1a3aa6 39
asaewing 0:61923a9dd110 40 DataStorage::_W1_A = 0;
asaewing 0:61923a9dd110 41 DataStorage::_W1_B = 0;
asaewing 0:61923a9dd110 42
asaewing 0:61923a9dd110 43 DataStorage::_W2_A = 0;
asaewing 0:61923a9dd110 44 DataStorage::_W2_B = 0;
asaewing 0:61923a9dd110 45
asaewing 0:61923a9dd110 46 DataStorage::_W3_A = 0;
asaewing 0:61923a9dd110 47 DataStorage::_W3_B = 0;
asaewing 0:61923a9dd110 48
asaewing 0:61923a9dd110 49 DataStorage::_W4_A = 0;
asaewing 0:61923a9dd110 50 DataStorage::_W4_B = 0;
asaewing 0:61923a9dd110 51 }
asaewing 0:61923a9dd110 52
asaewing 0:61923a9dd110 53 void WheelManager::moveDirectionControl(int tmpMoveDirection) {
asaewing 0:61923a9dd110 54 _moveDirection = tmpMoveDirection;
asaewing 0:61923a9dd110 55
asaewing 0:61923a9dd110 56 //W1 W4//
asaewing 0:61923a9dd110 57 if (tmpMoveDirection>=5 && tmpMoveDirection<=9 || tmpMoveDirection==3 || tmpMoveDirection==12){
asaewing 0:61923a9dd110 58 DataStorage::_W1_A = 0;
asaewing 0:61923a9dd110 59 DataStorage::_W1_B = 1;
asaewing 0:61923a9dd110 60
asaewing 0:61923a9dd110 61 DataStorage::_W4_A = 1;
asaewing 0:61923a9dd110 62 DataStorage::_W4_B = 0;
asaewing 1:b794ac1a3aa6 63
asaewing 1:b794ac1a3aa6 64 //DataStorage::set_PWM_status(1, 0, 1);
asaewing 1:b794ac1a3aa6 65 //DataStorage::set_PWM_status(4, 1, 0);
asaewing 0:61923a9dd110 66 } else if (tmpMoveDirection>=13 && tmpMoveDirection<=15 || tmpMoveDirection==0 || tmpMoveDirection==1 || tmpMoveDirection==4 || tmpMoveDirection==11){
asaewing 0:61923a9dd110 67 DataStorage::_W1_A = 1;
asaewing 0:61923a9dd110 68 DataStorage::_W1_B = 0;
asaewing 0:61923a9dd110 69
asaewing 0:61923a9dd110 70 DataStorage::_W4_A = 0;
asaewing 0:61923a9dd110 71 DataStorage::_W4_B = 1;
asaewing 1:b794ac1a3aa6 72 //DataStorage::set_PWM_status(1, 1, 0);
asaewing 1:b794ac1a3aa6 73 //DataStorage::set_PWM_status(4, 0, 1);
asaewing 0:61923a9dd110 74 } else if (tmpMoveDirection==2 || tmpMoveDirection==10){
asaewing 0:61923a9dd110 75 DataStorage::_W1_A = 0;
asaewing 0:61923a9dd110 76 DataStorage::_W1_B = 0;
asaewing 0:61923a9dd110 77
asaewing 0:61923a9dd110 78 DataStorage::_W4_A = 0;
asaewing 0:61923a9dd110 79 DataStorage::_W4_B = 0;
asaewing 1:b794ac1a3aa6 80 //DataStorage::set_PWM_status(1, 0, 0);
asaewing 1:b794ac1a3aa6 81 //DataStorage::set_PWM_status(4, 0, 0);
asaewing 0:61923a9dd110 82 }
asaewing 0:61923a9dd110 83
asaewing 0:61923a9dd110 84 //W2 W3//
asaewing 0:61923a9dd110 85 if (tmpMoveDirection>=0 && tmpMoveDirection<=3 || tmpMoveDirection==5 || tmpMoveDirection==12 || tmpMoveDirection==15){
asaewing 0:61923a9dd110 86 DataStorage::_W2_A = 0;
asaewing 0:61923a9dd110 87 DataStorage::_W2_B = 1;
asaewing 0:61923a9dd110 88
asaewing 0:61923a9dd110 89 DataStorage::_W3_A = 1;
asaewing 0:61923a9dd110 90 DataStorage::_W3_B = 0;
asaewing 1:b794ac1a3aa6 91 //DataStorage::set_PWM_status(2, 0, 1);
asaewing 1:b794ac1a3aa6 92 //DataStorage::set_PWM_status(3, 1, 0);
asaewing 0:61923a9dd110 93 } else if (tmpMoveDirection>=7 && tmpMoveDirection<=11 || tmpMoveDirection==4 || tmpMoveDirection==13){
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 = 0;
asaewing 0:61923a9dd110 98 DataStorage::_W3_B = 1;
asaewing 1:b794ac1a3aa6 99 //DataStorage::set_PWM_status(2, 1, 0);
asaewing 1:b794ac1a3aa6 100 //DataStorage::set_PWM_status(3, 0, 1);
asaewing 0:61923a9dd110 101 } else if (tmpMoveDirection==6 || tmpMoveDirection==14){
asaewing 0:61923a9dd110 102 DataStorage::_W2_A = 0;
asaewing 0:61923a9dd110 103 DataStorage::_W2_B = 0;
asaewing 0:61923a9dd110 104
asaewing 0:61923a9dd110 105 DataStorage::_W3_A = 0;
asaewing 0:61923a9dd110 106 DataStorage::_W3_B = 0;
asaewing 1:b794ac1a3aa6 107 //DataStorage::set_PWM_status(2, 0, 0);
asaewing 1:b794ac1a3aa6 108 //DataStorage::set_PWM_status(3, 0, 0);
asaewing 0:61923a9dd110 109 }
asaewing 0:61923a9dd110 110
asaewing 0:61923a9dd110 111 //W1 W2 W3 W4//
asaewing 0:61923a9dd110 112 if (tmpMoveDirection==16){
asaewing 0:61923a9dd110 113 DataStorage::_W1_A = 1;
asaewing 0:61923a9dd110 114 DataStorage::_W1_B = 0;
asaewing 0:61923a9dd110 115
asaewing 0:61923a9dd110 116 DataStorage::_W2_A = 1;
asaewing 0:61923a9dd110 117 DataStorage::_W2_B = 0;
asaewing 0:61923a9dd110 118
asaewing 0:61923a9dd110 119 DataStorage::_W3_A = 1;
asaewing 0:61923a9dd110 120 DataStorage::_W3_B = 0;
asaewing 0:61923a9dd110 121
asaewing 0:61923a9dd110 122 DataStorage::_W4_A = 1;
asaewing 0:61923a9dd110 123 DataStorage::_W4_B = 0;
asaewing 1:b794ac1a3aa6 124 /*DataStorage::set_PWM_status(1, 1, 0);
asaewing 1:b794ac1a3aa6 125 DataStorage::set_PWM_status(2, 1, 0);
asaewing 1:b794ac1a3aa6 126 DataStorage::set_PWM_status(3, 1, 0);
asaewing 1:b794ac1a3aa6 127 DataStorage::set_PWM_status(4, 1, 0);*/
asaewing 0:61923a9dd110 128 } else if (tmpMoveDirection==17){
asaewing 0:61923a9dd110 129 DataStorage::_W1_A = 0;
asaewing 0:61923a9dd110 130 DataStorage::_W1_B = 1;
asaewing 0:61923a9dd110 131
asaewing 0:61923a9dd110 132 DataStorage::_W2_A = 0;
asaewing 0:61923a9dd110 133 DataStorage::_W2_B = 1;
asaewing 0:61923a9dd110 134
asaewing 0:61923a9dd110 135 DataStorage::_W3_A = 0;
asaewing 0:61923a9dd110 136 DataStorage::_W3_B = 1;
asaewing 0:61923a9dd110 137
asaewing 0:61923a9dd110 138 DataStorage::_W4_A = 0;
asaewing 0:61923a9dd110 139 DataStorage::_W4_B = 1;
asaewing 1:b794ac1a3aa6 140 /*DataStorage::set_PWM_status(1, 0, 1);
asaewing 1:b794ac1a3aa6 141 DataStorage::set_PWM_status(2, 0, 1);
asaewing 1:b794ac1a3aa6 142 DataStorage::set_PWM_status(3, 0, 1);
asaewing 1:b794ac1a3aa6 143 DataStorage::set_PWM_status(4, 0, 1);*/
asaewing 0:61923a9dd110 144 } else if (tmpMoveDirection==40){
asaewing 0:61923a9dd110 145 DataStorage::_W1_A = 0;
asaewing 0:61923a9dd110 146 DataStorage::_W1_B = 0;
asaewing 0:61923a9dd110 147
asaewing 0:61923a9dd110 148 DataStorage::_W2_A = 0;
asaewing 0:61923a9dd110 149 DataStorage::_W2_B = 0;
asaewing 0:61923a9dd110 150
asaewing 0:61923a9dd110 151 DataStorage::_W3_A = 0;
asaewing 0:61923a9dd110 152 DataStorage::_W3_B = 0;
asaewing 0:61923a9dd110 153
asaewing 0:61923a9dd110 154 DataStorage::_W4_A = 0;
asaewing 0:61923a9dd110 155 DataStorage::_W4_B = 0;
asaewing 1:b794ac1a3aa6 156 /*DataStorage::set_PWM_status(1, 0, 0);
asaewing 1:b794ac1a3aa6 157 DataStorage::set_PWM_status(2, 0, 0);
asaewing 1:b794ac1a3aa6 158 DataStorage::set_PWM_status(3, 0, 0);
asaewing 1:b794ac1a3aa6 159 DataStorage::set_PWM_status(4, 0, 0);*/
asaewing 0:61923a9dd110 160 }
asaewing 0:61923a9dd110 161 }
asaewing 0:61923a9dd110 162
asaewing 0:61923a9dd110 163 void WM_pwmControl(DataStorage* tmpDS) {
asaewing 0:61923a9dd110 164 printf("### WM pwmControl - Start\r\n");
asaewing 0:61923a9dd110 165
asaewing 0:61923a9dd110 166
asaewing 0:61923a9dd110 167 float *tmpPeriod2 = new float[4];
asaewing 0:61923a9dd110 168 float *tmpWrite2 = new float[4];
asaewing 1:b794ac1a3aa6 169
asaewing 1:b794ac1a3aa6 170 for (int ii=0;ii<4;ii++) {
asaewing 1:b794ac1a3aa6 171 tmpPeriod2[ii] = (float)(*tmpDS).WM_PWM[ii+1][0]/1000000.0f;
asaewing 1:b794ac1a3aa6 172 tmpWrite2[ii] = (float)(*tmpDS).WM_PWM[ii+1][1]/100.0f;
asaewing 1:b794ac1a3aa6 173 }
asaewing 1:b794ac1a3aa6 174 /*tmpPeriod2[0] = (float)(*tmpDS).WM_PWM[1][0]/1000000.0f;
asaewing 0:61923a9dd110 175 tmpPeriod2[1] = (float)(*tmpDS).WM_PWM[2][0]/1000000.0f;
asaewing 0:61923a9dd110 176 tmpPeriod2[2] = (float)(*tmpDS).WM_PWM[3][0]/1000000.0f;
asaewing 0:61923a9dd110 177 tmpPeriod2[3] = (float)(*tmpDS).WM_PWM[4][0]/1000000.0f;
asaewing 0:61923a9dd110 178
asaewing 0:61923a9dd110 179 tmpWrite2[0] = (float)(*tmpDS).WM_PWM[1][1]/100.0f;
asaewing 0:61923a9dd110 180 tmpWrite2[1] = (float)(*tmpDS).WM_PWM[2][1]/100.0f;
asaewing 0:61923a9dd110 181 tmpWrite2[2] = (float)(*tmpDS).WM_PWM[3][1]/100.0f;
asaewing 1:b794ac1a3aa6 182 tmpWrite2[3] = (float)(*tmpDS).WM_PWM[4][1]/100.0f;*/
asaewing 0:61923a9dd110 183
asaewing 1:b794ac1a3aa6 184 printf("### WM pwmControl - 01 tmpPeriod2- %f; %f; %f; %f;\r\n", tmpPeriod2[0], tmpPeriod2[1], tmpPeriod2[2], tmpPeriod2[3]);
asaewing 0:61923a9dd110 185
asaewing 1:b794ac1a3aa6 186 //for (int ii=0;ii<4;ii++) tmpDS->set_PWM_period(ii, tmpPeriod2[ii]);
asaewing 0:61923a9dd110 187 (tmpDS->_WM_PWM_out01).period(tmpPeriod2[0]);
asaewing 0:61923a9dd110 188 (tmpDS->_WM_PWM_out02).period(tmpPeriod2[1]);
asaewing 0:61923a9dd110 189 (tmpDS->_WM_PWM_out03).period(tmpPeriod2[2]);
asaewing 0:61923a9dd110 190 (tmpDS->_WM_PWM_out04).period(tmpPeriod2[3]);
asaewing 0:61923a9dd110 191
asaewing 1:b794ac1a3aa6 192 printf("### WM pwmControl - 02 tmpWrite2 - %f; %f; %f; %f;\r\n", tmpWrite2[0], tmpWrite2[1], tmpWrite2[2], tmpWrite2[3]);
asaewing 0:61923a9dd110 193
asaewing 1:b794ac1a3aa6 194 //for (int ii=0;ii<4;ii++) tmpDS->set_PWM_write(ii, tmpWrite2[ii]);
asaewing 0:61923a9dd110 195 (tmpDS->_WM_PWM_out01).write(tmpWrite2[0]);
asaewing 0:61923a9dd110 196 (tmpDS->_WM_PWM_out02).write(tmpWrite2[1]);
asaewing 0:61923a9dd110 197 (tmpDS->_WM_PWM_out03).write(tmpWrite2[2]);
asaewing 0:61923a9dd110 198 (tmpDS->_WM_PWM_out04).write(tmpWrite2[3]);
asaewing 0:61923a9dd110 199
asaewing 0:61923a9dd110 200 delete [] tmpPeriod2;
asaewing 0:61923a9dd110 201 delete [] tmpWrite2;
asaewing 0:61923a9dd110 202
asaewing 0:61923a9dd110 203 printf("### WM pwmControl - End\r\n");
asaewing 0:61923a9dd110 204 }
asaewing 0:61923a9dd110 205
asaewing 0:61923a9dd110 206 /*void WheelManager::runControl(int tmpMoveDirection, int tmpPeriod, int tmpCycle) {
asaewing 0:61923a9dd110 207 moveDirectionControl(tmpMoveDirection);
asaewing 0:61923a9dd110 208 //pwmControl(1, tmpPeriod, tmpCycle);
asaewing 0:61923a9dd110 209 threadWM.start(callback(test, tmpMoveDirection));
asaewing 0:61923a9dd110 210 wait(1);
asaewing 0:61923a9dd110 211 }*/
asaewing 0:61923a9dd110 212
asaewing 0:61923a9dd110 213 void WheelManager::runControl(DataStorage* tmpDS, int** tmp) {
asaewing 0:61923a9dd110 214 printf("### WM runControl - Start\r\n");
asaewing 0:61923a9dd110 215
asaewing 0:61923a9dd110 216 /*PwmOut _W1_P(PD_14);
asaewing 0:61923a9dd110 217 PwmOut _W2_P(PD_15);
asaewing 0:61923a9dd110 218 PwmOut _W3_P(PE_12);
asaewing 0:61923a9dd110 219 PwmOut _W4_P(PE_14);*/
asaewing 0:61923a9dd110 220
asaewing 0:61923a9dd110 221 //PwmOut* _W_P[4] = {&_W1_P, &_W2_P, &_W3_P, &_W4_P};
asaewing 0:61923a9dd110 222
asaewing 0:61923a9dd110 223 for (int ii=0;ii<5;ii++){
asaewing 0:61923a9dd110 224 for (int jj=0;jj<2;jj++){
asaewing 0:61923a9dd110 225 DataStorage::WM_PWM[ii][jj] = tmp[ii][jj];
asaewing 0:61923a9dd110 226 }
asaewing 0:61923a9dd110 227 }
asaewing 0:61923a9dd110 228
asaewing 0:61923a9dd110 229
asaewing 0:61923a9dd110 230
asaewing 0:61923a9dd110 231 //printf("### runControl - 01 - 01 - %d\r\n", DataStorage::testInt1);
asaewing 0:61923a9dd110 232 //printf("### runControl - 01 - 02 - %d\r\n", DataStorage::testInt2);
asaewing 0:61923a9dd110 233 //printf("### runControl - 01 - 03 - %d\r\n", DataStorage::testInt3);
asaewing 0:61923a9dd110 234 //printf("### runControl - 01 - 01\r\n");
asaewing 0:61923a9dd110 235
asaewing 0:61923a9dd110 236 this->moveDirectionControl(tmp[0][0]);
asaewing 0:61923a9dd110 237
asaewing 0:61923a9dd110 238 //pwmControl(tmp[0], tmp[1], tmp[2]);
asaewing 0:61923a9dd110 239 //printf("### runControl - 01 - 02\r\n");
asaewing 0:61923a9dd110 240
asaewing 0:61923a9dd110 241 //PwmOut testPWM(PD_14);
asaewing 0:61923a9dd110 242 //printf("### runControl - 02\r\n");
asaewing 0:61923a9dd110 243 //(*tmpThread).terminate();
asaewing 0:61923a9dd110 244
asaewing 0:61923a9dd110 245 //printf("### runControl - 03\r\n");
asaewing 0:61923a9dd110 246 //(*tmpThread).start(callback(WM_pwmControl, tmpDS));
asaewing 0:61923a9dd110 247 WM_pwmControl(tmpDS);
asaewing 0:61923a9dd110 248
asaewing 0:61923a9dd110 249 this->moveDirectionControl(tmp[0][0]);
asaewing 0:61923a9dd110 250
asaewing 0:61923a9dd110 251 //printf("### runControl - 04\r\n");
asaewing 0:61923a9dd110 252 //wait(1);
asaewing 0:61923a9dd110 253
asaewing 0:61923a9dd110 254 //printf("### runControl - 05\r\n");
asaewing 0:61923a9dd110 255 //(*tmpThread).join();
asaewing 0:61923a9dd110 256
asaewing 0:61923a9dd110 257 printf("### WM runControl - End\r\n");
asaewing 0:61923a9dd110 258 }
asaewing 0:61923a9dd110 259
asaewing 0:61923a9dd110 260 void WheelManager::runControl2(DataStorage* tmpDS, int** tmp) {
asaewing 0:61923a9dd110 261 printf("### WM runControl2 - Start\r\n");
asaewing 0:61923a9dd110 262
asaewing 0:61923a9dd110 263 for (int ii=0;ii<5;ii++){
asaewing 0:61923a9dd110 264 for (int jj=0;jj<2;jj++){
asaewing 0:61923a9dd110 265 DataStorage::WM_PWM[ii][jj] = tmp[ii][jj];
asaewing 0:61923a9dd110 266 }
asaewing 0:61923a9dd110 267 }
asaewing 0:61923a9dd110 268
asaewing 0:61923a9dd110 269 this->moveDirectionControl(tmp[0][0]);
asaewing 0:61923a9dd110 270
asaewing 0:61923a9dd110 271 WM_pwmControl(tmpDS);
asaewing 0:61923a9dd110 272
asaewing 0:61923a9dd110 273 this->moveDirectionControl(tmp[0][0]);
asaewing 0:61923a9dd110 274
asaewing 0:61923a9dd110 275 printf("### WM runControl2 - End\r\n");
asaewing 0:61923a9dd110 276 }
asaewing 0:61923a9dd110 277
asaewing 0:61923a9dd110 278 void WheelManager::runControlStop(DataStorage* tmpDS) {
asaewing 0:61923a9dd110 279 printf("### WM runControlStop - Start\r\n");
asaewing 0:61923a9dd110 280
asaewing 0:61923a9dd110 281 for (int ii=0;ii<5;ii++){
asaewing 0:61923a9dd110 282 DataStorage::WM_PWM[ii][0] = 1;
asaewing 0:61923a9dd110 283 DataStorage::WM_PWM[ii][1] = 0;
asaewing 0:61923a9dd110 284 }
asaewing 0:61923a9dd110 285
asaewing 0:61923a9dd110 286 this->moveDirectionControl(40);
asaewing 0:61923a9dd110 287
asaewing 0:61923a9dd110 288 WM_pwmControl(tmpDS);
asaewing 0:61923a9dd110 289
asaewing 0:61923a9dd110 290 this->moveDirectionControl(40);
asaewing 0:61923a9dd110 291
asaewing 0:61923a9dd110 292 printf("### WM runControlStop - End\r\n");
asaewing 0:61923a9dd110 293 }