Callum and Adel's changes on 12/02/19
Dependencies: Crypto
Diff: main.cpp
- Revision:
- 24:be5fef3dace1
- Parent:
- 23:ab1cb51527d1
- Child:
- 25:995865498aee
- Child:
- 28:4f02ac845e5d
--- a/main.cpp Sat Mar 16 14:06:41 2019 +0000 +++ b/main.cpp Sat Mar 16 16:43:47 2019 +0000 @@ -72,39 +72,9 @@ DigitalOut L3H(L3Hpin); PwmOut pwmCtrl(PWMpin); -/*//Declare and start threads -class T_{ - // protected: - public: - - Thread *p_comm_in; - Thread *p_comm_out; - Thread *p_motor_ctrl; - - - - T_(){ - //(priority, stack size, - Thread comm_in(osPriorityAboveNormal, 1024); - Thread comm_out(osPriorityAboveNormal, 1024); - Thread motor_ctrl(osPriorityAboveNormal, 1024); - - p_comm_in = &comm_in; - p_comm_out = &comm_out; - p_motor_ctrl = &motor_ctrl; - - } - - ~T_(){ - if (p_comm_in->get_state() == 2) - p_comm_in->terminate(); - if (p_comm_out->get_state() == 2) - p_comm_out->terminate(); - if (p_motor_ctrl->get_state() == 2) - p_motor_ctrl->terminate(); - } -};*/ +uint8_t stateCount[3]; +uint8_t theStates[3]; class Comm /*: public T_*/{ @@ -299,9 +269,30 @@ void motorCtrlFn() { Ticker motorCtrlTicker; motorCtrlTicker.attach_us(callback(this,&Comm::motorCtrlTick), 1e5); - while (1) { + uint8_t cpyStateCount[3]; + uint8_t cpyCurrentState; + while (_RUN) { t_motor_ctrl.signal_wait((int32_t)0x1); - pc.printf("B4115"); + core_util_critical_section_enter(); + //Access shared variables here + std::copy(stateCount, stateCount+3, cpyStateCount); + // TODO: A thing + cpyCurrentState = 0; + for (int i = 0; i < 3; ++i) { + stateCount[i] = 0; + } + core_util_critical_section_exit(); + + uint8_t iterElementMax = std::max_element(cpyStateCount, cpyStateCount+3) - cpyStateCount; + + int16_t totalDegrees = 360 * cpyStateCount[iterElementMax]; + int16_t stateDiff = theStates[iterElementMax]-cpyCurrentState; + if (stateDiff >= 0) { + totalDegrees = totalDegrees + (60* stateDiff); + } else { + totalDegrees = totalDegrees + (60*stateDiff*-1); + } + pc.printf("%u,%u,%u,%u. %.6i \r", iterElementMax, cpyStateCount[0],cpyStateCount[1],cpyStateCount[2], (totalDegrees*10)); } } @@ -320,7 +311,7 @@ Comm() : pc(SERIAL_TX, SERIAL_RX), t_comm_out(osPriorityAboveNormal, 1024), - t_motor_ctrl(osPriorityAboveNormal, 1024) + t_motor_ctrl(osPriorityAboveNormal2, 1024) { // inherit from the RawSerial constructor pc.printf("%s\n\r", "Welcome" ); @@ -455,6 +446,18 @@ int8_t currentState = *params[0]; int8_t offset = *params[1]; + switch (currentState) { + case 1: + stateCount[0]++; + break; + case (1 + lead): + stateCount[1]++; + break; + case (1 + (lead*2)): + stateCount[2]++; + break; + } + motorOut((currentState - offset + lead + 6) % 6); } @@ -497,6 +500,10 @@ //Run the motor synchronisation orState = motorHome(); + theStates[0] = orState; + theStates[1] = (orState + lead) % 6; + theStates[2] = (orState + (lead*2)) % 6; + // Add callbacks // I1.fall(&stateUpdate); // I2.fall(&stateUpdate); @@ -567,5 +574,4 @@ timer.reset(); } } - } \ No newline at end of file