Callum and Adel's changes on 12/02/19
Dependencies: Crypto
Diff: main.cpp
- Revision:
- 43:a6d20109b2f2
- Parent:
- 42:121148278dae
- Child:
- 44:990b5aaf5198
- Child:
- 45:402a8a9423b9
diff -r 121148278dae -r a6d20109b2f2 main.cpp --- a/main.cpp Wed Mar 20 16:41:44 2019 +0000 +++ b/main.cpp Wed Mar 20 19:53:12 2019 +0000 @@ -2,7 +2,7 @@ Change: Indx newCmd - MAXCMDLENGTH + _MAXCMDLENGTH move the global variables to a class because we arent paeasents - Mission Failed use jack's motor motor position fix class variable naming @@ -88,24 +88,23 @@ public: - RawSerial pc; - Thread t_comm_out; - bool _RUN; + volatile bool _outMining; + volatile float _targetVel, _targetRot; - int8_t modeBitfield; // 0,0,0,... <=> Torque,Rotation,Velocity - volatile uint8_t inCharIndex, cmdIndx, MAXCMDLENGTH; // - volatile uint32_t motorPower; // Motor Toque - volatile uint64_t newKey; // hash key - Mutex newKey_mutex; // Restrict access to prevent deadlock. + volatile int8_t _modeBitField; // 0,0,0,... <=> Melody,Torque,Rotation,Velocity + const uint8_t _MAXCMDLENGTH; // + volatile uint8_t _inCharIndex, _cmdIndex; // + volatile uint32_t _motorTorque; // Motor Toque + volatile uint64_t _newKey; // hash key + Mutex _newKeyMutex; // Restrict access to prevent deadlock. + + RawSerial _pc; + Thread _t_comm_out; + bool _RUN; - volatile float targetVel, targetRot; - volatile bool outMining; - - static const char MsgChar[11]; - - enum msgType { motorState, posIn, velIn, posOut, velOut, + enum msgType { motorState, posIn, velIn, posOut, velOut, hashRate, keyAdded, nonceMatch, - torque, rotations, + torque, rotations, melody, error}; typedef struct { @@ -118,60 +117,60 @@ //public: //--------- Default Constructor With Inheritance From RawSerial Constructor ---------// - Comm(): pc(SERIAL_TX, SERIAL_RX), t_comm_out(osPriorityAboveNormal, 1024){ + Comm(): _pc(SERIAL_TX, SERIAL_RX), _t_comm_out(osPriorityAboveNormal, 1024), _MAXCMDLENGTH(18){ - pc.printf("%s\n\r", "Welcome" ); - MAXCMDLENGTH = 18; + _pc.printf("\n\r%s\n\r", "Welcome" ); + // _MAXCMDLENGTH = 18; - pc.putc('>'); - for (int i = 0; i < MAXCMDLENGTH; ++i) { // reset buffer - inCharQ[i] = '.'; // MbedOS prints 'Embedded Systems are fun and do awesome things!' - pc.putc('.'); // if you print a null terminator + _pc.putc('>'); + for (int i = 0; i < _MAXCMDLENGTH; ++i) { // reset buffer + inCharQ[i] = (char)'.'; // MbedOS prints 'Embedded Systems are fun and do awesome things!' + _pc.putc('.'); // if you print a null terminator } - pc.putc('<'); - pc.putc('\r'); + _pc.putc('<'); _pc.putc('\r'); _pc.putc('>'); - inCharQ[MAXCMDLENGTH] = '\0'; - strncpy(newCmd, inCharQ, MAXCMDLENGTH); + inCharQ[_MAXCMDLENGTH] = (char)'\0'; + sprintf(inCharQ, "%s", inCharQ); // sorts out the correct string correctly + strncpy(newCmd, inCharQ, _MAXCMDLENGTH); - cmdIndx = 0; + _cmdIndex = 0; - inCharIndex = 0; - outMining = false; - pc.attach(callback(this, &Comm::serialISR)); + _inCharIndex = 0; + _outMining = false; + _pc.attach(callback(this, &Comm::serialISR)); - motorPower = 300; - targetVel = 45.0; - targetRot = 459.0; + _motorTorque = 300; + _targetVel = 45.0; + _targetRot = 459.0; - modeBitfield = 0x01; // Default is velocity mode + _modeBitField = 0x01; // Default is velocity mode } //--------- Interrupt Service Routine for Serial Port and Character Queue Handling ---------// void serialISR(){ - if (pc.readable()) { - char newChar = pc.getc(); + if (_pc.readable()) { + char newChar = _pc.getc(); - if (inCharIndex == (MAXCMDLENGTH)) { - inCharQ[MAXCMDLENGTH] = '\0'; // force the string to have an end character + if (_inCharIndex == (_MAXCMDLENGTH)) { + inCharQ[_MAXCMDLENGTH] = '\0'; // force the string to have an end character putMessage(error, 1); - inCharIndex = 0; // reset buffer index + _inCharIndex = 0; // reset buffer index } else{ if(newChar != '\r'){ //While the command is not over, - inCharQ[inCharIndex] = newChar; //save input character and - inCharIndex++; //advance index - pc.putc(newChar); + inCharQ[_inCharIndex] = newChar; //save input character and + _inCharIndex++; //advance index + _pc.putc(newChar); } else{ - inCharQ[inCharIndex] = '\0'; //When the command is finally over, - strncpy(newCmd, inCharQ, MAXCMDLENGTH); // Will copy 18 characters from inCharQ to newCmd + inCharQ[_inCharIndex] = '\0'; //When the command is finally over, + strncpy(newCmd, inCharQ, _MAXCMDLENGTH); // Will copy 18 characters from inCharQ to newCmd cmdParser(); //parse the command for decoding. - for (int i = 0; i < MAXCMDLENGTH; ++i) // reset buffer + for (int i = 0; i < _MAXCMDLENGTH; ++i) // reset buffer inCharQ[i] = ' '; - inCharIndex = 0; // reset index + _inCharIndex = 0; // reset index } } } @@ -179,46 +178,105 @@ //--------- Reset Cursor Position ---------// void returnCursor() { - pc.putc('>'); - for (int i = 0; i < inCharIndex; ++i) - pc.putc(inCharQ[i]); + _pc.putc('>'); + for (int i = 0; i < _inCharIndex; ++i) + _pc.putc(inCharQ[i]); } //--------- Parse Incomming Data From Serial Port ---------// void cmdParser(){ switch(newCmd[0]) { - case 'K': //(MsgChar[keyAdded]) - newKey_mutex.lock(); //Ensure there is no deadlock - sscanf(newCmd, "K%x", &newKey); //Find desired the Key code - putMessage(keyAdded, newKey); //Print it out - newKey_mutex.unlock(); + case 'K': //keyAdded + _newKeyMutex.lock(); //Ensure there is no deadlock + sscanf(newCmd, "K%x", &_newKey); //Find desired the Key code + putMessage(keyAdded, _newKey); //Print it out + _newKeyMutex.unlock(); + break; + + case 'V': //velIn + sscanf(newCmd, "V%f", &_targetVel); //Find desired the target velocity + _modeBitField = 0x01; //Adjust bitfield pos 1 + putMessage(velIn, _targetVel); //Print it out break; - case 'V': //(MsgChar[velIn]):// - sscanf(newCmd, "V%f", &targetVel); //Find desired the target velocity - modeBitfield = 0x01; //Adjust bitfield pos 1 - putMessage(velIn, targetVel); //Print it out + case 'R': //posIn + sscanf(newCmd, "R%f", &_targetRot); //Find desired target rotation + _modeBitField = 0x02; //Adjust bitfield pos 2 + putMessage(posIn, _targetRot); //Print it out + break; + + case 'x': //torque + sscanf(newCmd, "x%u", &_motorTorque); //Find desired target torque + _modeBitField = 0x04; //Adjust bitfield pos 3 + putMessage(torque, _motorTorque); //Print it out + break; + + case 'M': //mining display toggle + int8_t miningTest; + sscanf(newCmd, "M%d", &miningTest); //display if input is 1 + if (miningTest == 1) + _outMining = true; + else + _outMining = false; break; - case 'R': //(MsgChar[posIn]):// - sscanf(newCmd, "R%f", &targetRot); //Find desired target rotation - modeBitfield = 0x02; //Adjust bitfield pos 2 - putMessage(posIn, targetRot); //Print it out - break; + // This guy ugly, maybe use a function + case 'T': // Tune/ melody + uint8_t dur[9]; // Note Durations + char notes[9]; // Actual notes + uint8_t len = 0; // Length of notes + + for (int i = 1; i < _MAXCMDLENGTH; ++i) { // Find first # + if (newCmd[i] == '#') { + len = i; + break; // stop at first # found + } + } + + if (len>0) { // Parse the input only if # found + uint8_t newLen = 2*(len+1)+1; + bool isChar = true; + char formatSpec[newLen]; + formatSpec[0]='T'; + for (int i = 1; i < newLen; i=i+2) { // Create a format spec based on length of input + formatSpec[i] = '%'; + if (isChar) // if character + formatSpec[i+1] = 'c'; + else + formatSpec[i+1] = 'u'; + isChar = !isChar; + } - case 'T': //(MsgChar[torque]):// - sscanf(newCmd, "T%u", &motorPower); //Find desired target torque - modeBitfield = 0x04; //Adjust bitfield pos 3 - putMessage(torque, motorPower); //Print it out - break; - - case 'M': //(MsgChar[torque]):// - int8_t miningTest; - sscanf(newCmd, "M%d", &miningTest); //Find desired target torque - if (miningTest == 1) - outMining = true; + formatSpec[newLen] = '\0'; + sprintf(formatSpec, "%s", formatSpec); // Set string format correctly + _pc.printf("%s\n", formatSpec ); + sscanf(newCmd, formatSpec, ¬es[0], &dur[0], + ¬es[1], &dur[1], + ¬es[2], &dur[2], + ¬es[3], &dur[3], + ¬es[4], &dur[4], + ¬es[5], &dur[5], + ¬es[6], &dur[6], + ¬es[7], &dur[7], + ¬es[8], &dur[8] + ); + _modeBitField = 0x08; + // putMessage(melody, newCmd); //Print it out + _pc.printf(formatSpec, notes[0], dur[0], \ + notes[1], dur[1], \ + notes[2], dur[2], \ + notes[3], dur[3], \ + notes[4], dur[4], \ + notes[5], dur[5], \ + notes[6], dur[6], \ + notes[7], dur[7], \ + notes[8], dur[8] \ + ); + } else - outMining = false; + putMessage(error, 2); // bad times + break; + break; default: @@ -235,44 +293,44 @@ //Case Switch to Choose Serial Output Based on Incoming Message Enum switch (pMessage->type) { case motorState: - pc.printf("\r>%s< The motor is currently in state %x\n\r", inCharQ, pMessage->message); + _pc.printf("\r>%s< The motor is currently in state %x\n\r", inCharQ, pMessage->message); break; case hashRate: - if (outMining) { - pc.printf("\r>%s< Mining: %.4u Hash/s\r", inCharQ, (uint32_t) pMessage->message); + if (_outMining) { + _pc.printf("\r>%s< Mining: %.4u Hash/s\r", inCharQ, (uint32_t) pMessage->message); returnCursor(); - outMining = false; + _outMining = false; } break; case nonceMatch: - pc.printf("\r>%s< Nonce found: %x\n\r", inCharQ, pMessage->message); + _pc.printf("\r>%s< Nonce found: %x\n\r", inCharQ, pMessage->message); returnCursor(); break; case keyAdded: - pc.printf("\r>%s< New Key Added:\t0x%016x\n\r", inCharQ, pMessage->message); + _pc.printf("\r>%s< New Key Added:\t0x%016x\n\r", inCharQ, pMessage->message); break; case torque: - pc.printf("\r>%s< Motor Torque set to:\t%d\n\r", inCharQ, (int32_t) pMessage->message); + _pc.printf("\r>%s< Motor Torque set to:\t%d\n\r", inCharQ, (int32_t) pMessage->message); break; case velIn: - pc.printf("\r>%s< Target Velocity set to:\t%.2f\n\r", inCharQ, targetVel); + _pc.printf("\r>%s< Target Velocity set to:\t%.2f\n\r", inCharQ, _targetVel); break; case velOut: - pc.printf("\r>%s< Current Velocity:\t%.2f States/sec\n\r", inCharQ, (float) ((int32_t) pMessage->message)); + _pc.printf("\r>%s< Current Velocity:\t%.2f States/sec\n\r", inCharQ, (float) ((int32_t) pMessage->message)); break; case posIn: - pc.printf("\r>%s< Target # Rotations:\t%.2f\n\r", inCharQ, (float) ((int32_t) pMessage->message)); + _pc.printf("\r>%s< Target # Rotations:\t%.2f\n\r", inCharQ, (float) ((int32_t) pMessage->message)); break; case posOut: - pc.printf("\r>%s< Current Position:\t%.2f\n\r", inCharQ, (float) ((int32_t) pMessage->message /*/ 6*/)); + _pc.printf("\r>%s< Current Position:\t%.2f\n\r", inCharQ, (float) ((int32_t) pMessage->message /*/ 6*/)); break; case error: - pc.printf("\r>%s< Debugging position:%x\n\r", inCharQ, pMessage->message); - for (int i = 0; i < MAXCMDLENGTH; ++i) // reset buffer + _pc.printf("\r>%s< Debugging position:%x\n\r", inCharQ, pMessage->message); + for (int i = 0; i < _MAXCMDLENGTH; ++i) // reset buffer inCharQ[i] = ' '; break; default: - pc.printf("\r>%s< Unknown Error. Message: %x\n\r", inCharQ, pMessage->message); + _pc.printf("\r>%s< Unknown Error. Message: %x\n\r", inCharQ, pMessage->message); break; } @@ -290,29 +348,15 @@ void start_comm(){ _RUN = true; - - // reset buffer - // MbedOS prints 'Embedded Systems are fun and do awesome things!' - // if you print a null terminator - pc.putc('>'); - for (int i = 0; i < MAXCMDLENGTH; ++i) { - inCharQ[i] = '.'; - pc.putc('.'); + for (int i = 0; i < _MAXCMDLENGTH; ++i) { // reset buffer + inCharQ[i] = (char)'.'; // MbedOS prints 'Embedded Systems are fun and do awesome things!' } - pc.putc('<'); - pc.putc('\r'); - inCharQ[MAXCMDLENGTH] = '\0'; - strncpy(newCmd, inCharQ, MAXCMDLENGTH); - - // returnCursor(); + inCharQ[_MAXCMDLENGTH] = (char)'\0'; + sprintf(inCharQ, "%s", inCharQ); // sorts out the correct string correctly + strncpy(newCmd, inCharQ, _MAXCMDLENGTH); - // t_comm_in.start(callback(this, &Comm::commInFn)); - // this::thread::wait() - // wait(1.0); - t_comm_out.start(callback(this, &Comm::commOutFn)); - - + _t_comm_out.start(callback(this, &Comm::commOutFn)); } @@ -400,7 +444,7 @@ // Start motor control thread t_motor_ctrl.start(callback(this, &Motor::motorCtrlFn)); - p_comm->pc.printf("origin=%i, theStates=[%i,%i,%i]\n", orState, theStates[0], theStates[1], theStates[2]); + p_comm->_pc.printf("origin=%i, theStates=[%i,%i,%i]\n\r", orState, theStates[0], theStates[1], theStates[2]); } @@ -514,8 +558,8 @@ t_motor_ctrl.signal_wait((int32_t)0x1); core_util_critical_section_enter(); - cpyModeBitfield = p_comm->modeBitfield; - // p_comm->modeBitfield = 0; // nah + cpyModeBitfield = p_comm->_modeBitField; + // p_comm->_modeBitField = 0; // nah //Access shared variables here std::copy(stateCount, stateCount+3, cpyStateCount); cpyCurrentState = currentState; @@ -548,12 +592,12 @@ else { totalDegrees = totalDegrees + (ting[1]*stateDiff*-1); } - //p_comm->pc.printf("%u,%u,%u,%u. %.6i \r", iterElementMax, cpyStateCount[0],cpyStateCount[1],cpyStateCount[2], (totalDegrees*10)); + //p_comm->_pc.printf("%u,%u,%u,%u. %.6i \r", iterElementMax, cpyStateCount[0],cpyStateCount[1],cpyStateCount[2], (totalDegrees*10)); if ((cpyModeBitfield & 0x01) | (cpyModeBitfield & 0x02)) { //~~~~~Speed controller~~~~~~ cur_speed = totalDegrees / time_diff; - sError = (p_comm->targetVel * 6) - abs(cur_speed); //Read global variable targetVel updated by interrupt and calculate error between target and reality + sError = (p_comm->_targetVel * 6) - abs(cur_speed); //Read global variable _targetVel updated by interrupt and calculate error between target and reality if (sError == -abs(cur_speed)) { //Check if user entered V0, Ys = MAXPWM_PRD; //and set the output to maximum as specified @@ -565,7 +609,7 @@ // } else if (cpyModeBitfield & 0x02) { //~~~~~Rotation control~~~~~~ - rError = (p_comm->targetRot)*6 - totalDegrees; //Read global variable targetRot updated by interrupt and calculate the rotation error. + rError = (p_comm->_targetRot)*6 - totalDegrees; //Read global variable _targetRot updated by interrupt and calculate the rotation error. Yr = Kp2*rError + Kd*(rError - rErrorOld); //Implement controller transfer function Ys= Kp*Er + Kd* (dEr/dt) rErrorOld = rError; //Update rotation error // if(rError < 0) //Use the sign of the error to set controller wrt direction of rotation @@ -591,12 +635,12 @@ torque = MAXPWM_PRD; //Set it to our max. } - p_comm->motorPower = torque; - pwmCtrl.pulsewidth_us(p_comm->motorPower); + p_comm->_motorTorque = torque; + pwmCtrl.pulsewidth_us(p_comm->_motorTorque); } if (cpyModeBitfield & 0x04) { // if it is in torque mode, do no math, just set pulsewidth - torque = (int32_t)p_comm->motorPower; + torque = (int32_t)p_comm->_motorTorque; if (oldTorque != torque) { if(torque < 0){ //Variable torque cannot be negative since it sets the PWM torque = -torque; //Hence we make the value positive, @@ -609,7 +653,7 @@ } p_comm->putMessage((Comm::msgType)8, torque); - p_comm->motorPower = torque; + p_comm->_motorTorque = torque; pwmCtrl.pulsewidth_us(torque); oldTorque = torque; } @@ -617,10 +661,10 @@ //else { // if not Torque mode //balls //} - // pwmCtrl.write((float)(p_comm->motorPower/MAXPWM_PRD)); - // p_comm->motorPower = torque; //Lastly, update global variable motorPower which is updated by interrupt - // p_comm->pc.printf("\t\t\t\t\t\t %i, %i, %i \r", torque, Ys, Yr); - //p_comm->pc.printf("%u,%u,%u,%u. %.6i \r", iterElementMax, cpyStateCount[0],cpyStateCount[1],cpyStateCount[2], (totalDegrees*10)); + // pwmCtrl.write((float)(p_comm->_motorTorque/MAXPWM_PRD)); + // p_comm->_motorTorque = torque; //Lastly, update global variable _motorTorque which is updated by interrupt + // p_comm->_pc.printf("\t\t\t\t\t\t %i, %i, %i \r", torque, Ys, Yr); + //p_comm->_pc.printf("%u,%u,%u,%u. %.6i \r", iterElementMax, cpyStateCount[0],cpyStateCount[1],cpyStateCount[2], (totalDegrees*10)); } } @@ -664,9 +708,9 @@ while (1) { // Mutex For Access Control - comm_port.newKey_mutex.lock(); - *key = comm_port.newKey; - comm_port.newKey_mutex.unlock(); + comm_port._newKeyMutex.lock(); + *key = comm_port._newKey; + comm_port._newKeyMutex.unlock(); // Compute Hash and Counter miner.computeHash(hash, sequence, length64);