Callum and Adel's changes on 12/02/19
Dependencies: Crypto
Diff: main.cpp
- Revision:
- 25:995865498aee
- Parent:
- 24:be5fef3dace1
- Child:
- 26:fb6151e5907d
diff -r be5fef3dace1 -r 995865498aee main.cpp --- a/main.cpp Sat Mar 16 16:43:47 2019 +0000 +++ b/main.cpp Sat Mar 16 17:39:22 2019 +0000 @@ -276,7 +276,7 @@ core_util_critical_section_enter(); //Access shared variables here std::copy(stateCount, stateCount+3, cpyStateCount); - // TODO: A thing + // TODO: A thing yes cpyCurrentState = 0; for (int i = 0; i < 3; ++i) { stateCount[i] = 0; @@ -285,15 +285,28 @@ uint8_t iterElementMax = std::max_element(cpyStateCount, cpyStateCount+3) - cpyStateCount; - int16_t totalDegrees = 360 * cpyStateCount[iterElementMax]; + int16_t ting[2] = {5,1}; // 360,60 (for degrees), 5,1 (for states) + int16_t totalDegrees = ting[0] * cpyStateCount[iterElementMax]; int16_t stateDiff = theStates[iterElementMax]-cpyCurrentState; if (stateDiff >= 0) { - totalDegrees = totalDegrees + (60* stateDiff); + totalDegrees = totalDegrees + (ting[1]* stateDiff); } else { - totalDegrees = totalDegrees + (60*stateDiff*-1); + totalDegrees = totalDegrees + (ting[1]*stateDiff*-1); } pc.printf("%u,%u,%u,%u. %.6i \r", iterElementMax, cpyStateCount[0],cpyStateCount[1],cpyStateCount[2], (totalDegrees*10)); + + //~~~~~Speed controller~~~~~~ + /* + sError = (targetVel * 6) - abs(totalDegrees)*10; //Read global variable targetVel updated by interrupt and calculate error between target and reality + int32_t Ys; //Initialise controller output Ys + if (sError == -abs(velocity)) { //Check if user entered V0, + Ys = MAXPWM; //and set the output to maximum as specified + } else { + Ys = (int)(Kp1 * sError); //If the user didn't enter V0 implement controller transfer function: Ys = Kp * (s -|v|) where, + } + */ } + } void motorCtrlTick(){