Internet of Tings
An embedded device
Dependencies: Crypto
Revision 25:ca5ccbf55b07, committed 2019-03-21
- Comitter:
- Olaffo
- Date:
- Thu Mar 21 16:53:23 2019 +0000
- Parent:
- 24:7dd4e187dd30
- Child:
- 26:d3e69e507616
- Commit message:
- fixed motor control, code cleaned up
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Wed Mar 20 23:46:05 2019 +0000
+++ b/main.cpp Thu Mar 21 16:53:23 2019 +0000
@@ -68,6 +68,7 @@
//int8_t intStateOld = 0;
int32_t revoCounter = 0; //Counts the number of revolutions
int32_t motorVelocity;
+uint8_t mode = 6;
//Phase lead to make motor spin
//int8_t lead = -2; //2 for forwards, -2 for backwards
@@ -78,7 +79,7 @@
} mail_t;
Mail<mail_t, 16> mail_box;
-Thread commandProcessorthread(osPriorityNormal,1024);
+Thread commsIn(osPriorityNormal,1024);
Thread bitcointhread(osPriorityNormal,1024);
Thread motorCtrlT(osPriorityNormal,1024);
Thread commsOut(osPriorityNormal,1024);
@@ -105,25 +106,28 @@
//CommsOut.h
enum message_keys {
-KEY_DECLINED = 0,
-ROTATION_CMD = 1,
-MAX_SPEED_CMD = 2,
-KEY_ECHO = 3,
-R_ECHO1 = 4,
-R_ECHO2 = 5,
-MAX_SPEED_ECHO1= 6,
-MAX_SPEED_ECHO2= 7,
+KEY_DECLINED = 0,
+ROTATION_CMD = 1,
+ROTATION_FF_CMD = 2,
+MAX_SPEED_CMD = 3,
+KEY_ECHO = 4,
+TUNE_CMD = 5,
+STANDARD = 6,
+FOREVER_FORWARD = 7,
+LEAD = 9,
-INVALID_CMD = 10,
-MOTOR_POS = 11,
-MOTOR_SPEED = 12,
+INVALID_CMD = 10,
+MOTOR_POS = 11,
+MOTOR_SPEED = 12,
-NONCE_DETECT = 14,
-ROTOR_ORG = 15,
+NONCE_DETECT = 14,
+ROTOR_ORG = 15,
-WELCOME = 20
+WELCOME = 20
+
+
};
void putMessage(int message, uint64_t data = 0, float fdata=0){
@@ -146,25 +150,24 @@
case ROTATION_CMD :
pc.printf("Rotation count: %3.2f\n\r", mail->fdata);
break;
- case MAX_SPEED_CMD :
- pc.printf("Max speed command\n\r");
+ case ROTATION_FF_CMD :
+ pc.printf("I will rotate forever...");
break;
- case MAX_SPEED_ECHO1 :
- pc.printf("Max speed command:\n\r");
- pc.printf("Max speed int: %d\n\r", mail->data);
- break;
- case MAX_SPEED_ECHO2 :
- pc.printf("Max speed decimal: %d\n\r", mail->data);
- break;
+ case MAX_SPEED_CMD :
+ pc.printf("Max speed: %2.1f\n\r", mail->fdata);
+ break;
case KEY_ECHO :
pc.printf("Received key: %016llx\n\r", mail->data);
+ break;
+ case TUNE_CMD :
+ pc.printf("Tune command!\n\r");
break;
case INVALID_CMD :
pc.printf("Invalid command!\r\n");
break;
case MOTOR_POS :
pc.printf("Motor position: %f\r\n", mail->fdata);
- //pc.printf("{TIMEPLOT|%.2f|Motor Position", mail->fdata);
+ //pc.printf("{TIMEPLOT|%.2f|Motor Position}", mail->fdata);
break;
case MOTOR_SPEED :
pc.printf("Motor speed: %f\r\n", mail->fdata);
@@ -173,10 +176,13 @@
pc.printf("Successful nonce: %016x\n\r", mail->data);
break;
case WELCOME :
- pc.printf("Hello Pete\n\r");
+ pc.printf("Hello Olaf\n\r");
break;
case ROTOR_ORG :
pc.printf("Rotor origin: %x\n\r", orState);
+ case LEAD :
+ pc.printf("Lead: %d\n\r\n\r", mail->data);
+ break;
}
mail_box.free(mail);
@@ -191,98 +197,94 @@
//CommsIn.h
-void commandDecoder(char* command) {
+void R_Decoder(char* command) {
int8_t sign =1;
uint8_t index = 1;
int intValue = 0;
float decimalValue = 0;
- switch (command[0]) {
- case 'R' :
- // Check sign
- if (command[1] == '-'){
- sign = -1;
- index++;
- }
- // Take first digit
- if (command[index] > ('0'-1) && command[index] < (1 + '9')) {
- intValue = command[index] - '0';
- index++;
- }
- else {
- putMessage(INVALID_CMD);
- break;
- }
+
+ // Check sign
+ if (command[1] == '-'){
+ sign = -1;
+ index++;
+ }
+ // Take first digit
+ if (command[index] > ('0'-1) && command[index] < (1 + '9')) {
+ intValue = command[index] - '0';
+ index++;
+ }
+ else {
+ putMessage(INVALID_CMD);
+ return;
+ }
- //Try taking 2 more digits
- if (command[index] > ('0'-1) && command[index] < (1 + '9')) {
- intValue = intValue*10 + command[index] - '0';
- index++;
- }
- if (command[index] > ('0'-1) && command[index] < (1 + '9')) {
- intValue = intValue*10 + command[index] - '0';
- index++;
- }
- //Check for '.'
- if (command[index] == '.') {
- index++;
- //Check for decimals
- if (command[index] > ('0'-1) && command[index] < (1 + '9')) {
- decimalValue = (command[index] - '0')/10;
- index++;
- }
- if (command[index] > ('0'-1) && command[index] < (1 + '9')) {
- decimalValue = (decimalValue/10 + command[index] - '0')/10;
- }
- }
- decimalValue = (decimalValue + intValue) * sign;
- putMessage(ROTATION_CMD,0,decimalValue);
- //HERE SEND IT TO ANY GLOBAL VARIABLE YOU WISH
-
- newRev = decimalValue;
- break;
-
- case 'V' :
- // Take first digit
- if (command[index] > ('0'-1) && command[index] < (1 + '9')) {
- intValue = command[index] - '0';
- index++;
- }
- else {
- putMessage(INVALID_CMD);
- break;
- }
+ //Try taking 2 more digits
+ if (command[index] > ('0'-1) && command[index] < (1 + '9')) {
+ intValue = intValue*10 + command[index] - '0';
+ index++;
+ }
+ if (command[index] > ('0'-1) && command[index] < (1 + '9')) {
+ intValue = intValue*10 + command[index] - '0';
+ index++;
+ }
+ //Check for '.'
+ if (command[index] == '.') {
+ index++;
+ //Check for decimals
+ if (command[index] > ('0'-1) && command[index] < (1 + '9')) {
+ decimalValue = (command[index] - '0')/10;
+ index++;
+ }
+ if (command[index] > ('0'-1) && command[index] < (1 + '9')) {
+ decimalValue = (decimalValue/10 + command[index] - '0')/10;
+ }
+ }
+ decimalValue = (decimalValue + intValue) * sign;
+ if (decimalValue == 0){
+ mode = FOREVER_FORWARD;
+ putMessage(ROTATION_FF_CMD);
+ }
+ else {
+ newRev = decimalValue;
+ putMessage(ROTATION_CMD,0,decimalValue);
+ }
+}
+
+void V_Decoder(char* command) {
+ uint8_t index = 1;
+ int intValue = 0;
+ float decimalValue = 0;
+
+ // Take first digit
+ if (command[index] > ('0'-1) && command[index] < (1 + '9')) {
+ intValue = command[index] - '0';
+ index++;
+ }
+ else {
+ putMessage(INVALID_CMD);
+ return;
+ }
- //Try taking 2 more digits
- if (command[index] > ('0'-1) && command[index] < (1 + '9')) {
- intValue = intValue*10 + command[index] - '0';
- index++;
- }
- if (command[index] > ('0'-1) && command[index] < (1 + '9')) {
- intValue = intValue*10 + command[index] - '0';
- index++;
- }
- //Check for '.'
- if (command[index] == '.') {
- index++;
- //Check for one decimal
- if (command[index] > ('0'-1) && command[index] < (1 + '9')) {
- decimalValue = (command[index] - '0')/10;
- }
- }
- putMessage(MAX_SPEED_ECHO1, intValue);
- putMessage(MAX_SPEED_ECHO2, (int) (100*decimalValue));
- decimalValue = (decimalValue + intValue);
- //HERE SEND IT TO ANY GLOBAL VARIABLE YOU WISH
- maxSpeed = decimalValue;
- break;
-
- case 'K' :
- ///pc.printf("Received key: %016llx\n\r", mail->data);
- break;
- case 'T' :
- //pc.printf("Received key: %016llx\n\r", mail->data);
- break;
+ //Try taking 2 more digits
+ if (command[index] > ('0'-1) && command[index] < (1 + '9')) {
+ intValue = intValue*10 + command[index] - '0';
+ index++;
+ }
+ if (command[index] > ('0'-1) && command[index] < (1 + '9')) {
+ intValue = intValue*10 + command[index] - '0';
+ index++;
+ }
+ //Check for '.'
+ if (command[index] == '.') {
+ index++;
+ //Check for one decimal
+ if (command[index] > ('0'-1) && command[index] < (1 + '9')) {
+ decimalValue = (command[index] - '0')/10;
+ }
}
+ decimalValue = (decimalValue + intValue);
+ maxSpeed = decimalValue;
+ putMessage(MAX_SPEED_CMD,0,decimalValue);
}
void commandProcessor() {
@@ -301,24 +303,18 @@
command[17] = '\0';
if (command[0] == 'R') {
- commandDecoder(command);
+ R_Decoder(command);
motorPosition_command = motorPosition;
- putMessage(ROTATION_CMD);
-
-
}
else if (command[0] == 'V') {
- commandDecoder(command);
- putMessage(MAX_SPEED_CMD);
+ V_Decoder(command);
}
else if (command[0] == 'K') {
if (index == 18){ // when index is 18 means you entered K and 16 digits
number = command +1; //super bad solution, but I don't know how to work with strings in C
receivedKey = strtoull(number, NULL, 16);
putMessage(KEY_ECHO,receivedKey);
- //receivedKey = 2147483648;
- //sscanf(command, "%d", &receivedKey);
- //pc.printf("Received key: %016llx\n\r", receivedKey);
+
newKey_mutex.lock();
newKey = receivedKey;
newKey_mutex.unlock();
@@ -327,8 +323,7 @@
};
}
else if (command[0] == 'T') {
- pc.printf("Melody command\n");
- pc.printf("%s", command);
+ putMessage(TUNE_CMD);
}
memset(command, 0, sizeof(command));
index = 0;
@@ -377,10 +372,7 @@
}
t.stop();
}
- }
-
-
-
+}
@@ -460,7 +452,7 @@
float Rev;
int8_t leads = -2;
int8_t leadr = -2;
- float kps = 80;
+ float kps = 25;
float kis = 0.07;
float kpr = 35;
float kdr = 14.75; //proportional constant for speed
@@ -475,6 +467,7 @@
while(1){
motorCtrlT.signal_wait(0x1);
+ // Convert variabls to int
Speed = maxSpeed*6;
Rev = newRev *6;
motorPos = motorPosition;
@@ -486,118 +479,132 @@
//equation for controls
sError = Speed -abs(motorVelocity);
- intError = intError + sError;
- Ts = (kps * sError + kis * intError)*(error-olderror/abs(error-olderror));
- Tr = kpr*error+kdr*(error-olderror)/ myTime;
- //pc.printf("Error: %f\n\r", Tr);
-
- // Lead flip
- if ( Ts >0){
- leads = 2;
+ if (motorVelocity != 0){
+ intError = intError + sError;
}
- else if ( Ts <0){
- leads = -2;
- }
-
- if (Tr >= 0){
- leadr= -2;
- }
- else if (Tr < 0){
- leadr = 2;
+ Ts = (kps * sError + kis * intError)*(error/abs(error));
+ Tr = kpr*error+kdr*(error-olderror)/ myTime;
+
+ // Speed AND rotation control (aka standard mode)
+ if (mode = STANDARD)
+ {
+ // Case for reverse rotation
+ if (error < 0){
+
+ // Case for choosing rotational control
+ if (Ts <=Tr ){
+
+ // Flip lead
+ if ( Tr >= 0) { lead = -2; }
+ else if ( Tr < 0) { lead = 2; }
+
+ // Assure torque is in bounds
+ if (Tr > 2000){ Tr = 2000; }
+ else if (Tr < 0){ Tr = -Tr; }
+ else if (Tr < -2000){ Tr = 2000; }
+
+ // Pass torque
+ pulseWidth = Tr;
+ }
+
+ // Case for choosing speed control
+ else {
+ pc.printf("Chose Ts %f\n\r", Ts);
+ // Flip lead
+ if ( Ts >= 0) { lead = -2; }
+ else if ( Ts < 0) { lead = 2; }
+
+ // Assure torque is in bounds
+ if (Ts > 2000){ Ts = 2000; }
+ else if (Ts < 0){ Ts = -Ts; }
+ else if (Ts < -2000){ Ts = 2000; }
+
+ // Pass torque
+ pulseWidth = Ts;
+ }
+ }
+
+ // Case for forward rotation
+ else if (error >= 0){
+
+ // Case for choosing speed control
+ if (Ts <=Tr ){
+ pc.printf("Chose Ts %f\n\r", Ts);
+ // Flip lead
+ if ( Ts >= 0) { lead = -2; }
+ else if ( Ts < 0) { lead = 2; }
+
+ // Assure torque is in bounds
+ if (Ts > 2000){ Ts = 2000; }
+ else if (Ts < 0){ Ts = -Ts; }
+ else if (Ts < -2000){ Ts = 2000; }
+
+ // Pass torque
+ pulseWidth = Ts;
+ }
+
+ // Case for rotational speed control
+ else {
+
+ // Flip lead
+ if ( Tr >= 0) { lead = -2; }
+ else if ( Tr < 0) { lead = 2; }
+
+ // Assure torque is in bounds
+ if (Tr > 2000){ Tr = 2000; }
+ else if (Tr < 0){ Tr = -Tr; }
+ else if (Tr < -2000){ Tr = 2000; }
+
+ // Pass torque
+ pulseWidth = Tr;
+ }
+ }
}
- //Torque adjustment
- if (Ts > 2000){
-
- Ts = 2000;
- }
- else if (Ts < 0){
-
- Ts = -Ts;
- }
- else if (Ts < -2000){
-
- Ts = 2000;
- }
- if (Tr > 2000){
-
- Tr = 2000;
- }
- else if (Tr < 0){
-
- Tr = -Tr;
- }
- else if (Tr < -2000){
-
- Tr = 2000;
- }
- ;
- //if (motorVelocity < 0){
- if (Ts <=Tr ){
- lead = leads;
- pulseWidth = Ts;
-
- }
- else{
- lead = leadr;
- pulseWidth = Tr;
-
- } /*
- }
- else{
- if (Ts <=Tr ){
- lead = leads;
+
+ // ONLY speed control (aka forever forward mode)
+ else if (mode = FOREVER_FORWARD){
+
+ if ( Ts >= 0) { lead = -2; }
+ else if ( Ts < 0) { lead = 2; }
+
+ // Assure torque is in bounds
+ if (Ts > 2000){ Ts = 2000; }
+ else if (Ts < 0){ Ts = -Ts; }
+ else if (Ts < -2000){ Ts = 2000; }
+
pulseWidth = Ts;
-
}
- else{
- lead = leadr;
- pulseWidth = Tr;
-
- }
- }*/
-
-
-
+ counter++;
motorTime.reset();
// Serial output to monitor speed and position
- counter++;
if(counter == 10){
counter = 0;
//display velocity and motor position
- // pc.printf ("motor Pos: %f\n\r", (motorPosition/6.0));
putMessage(MOTOR_POS,0,(float)(motorPosition/6.0));
putMessage(MOTOR_SPEED,0,(float)(motorVelocity/6.0));
- //pc.printf ("torque: %f\n\r", Ts);
+ putMessage(LEAD,lead);
}
olderror=error;
}
}
int main() {
- //Serial pc(SERIAL_TX, SERIAL_RX);
-
//Initialise bincoin mining and communication
-
-
-
-
+ // Start up checkup
d9.period(0.002f); //Set PWM period in seconds
+ int8_t orState = motorHome();
+ putMessage(WELCOME);
+ putMessage(ROTOR_ORG);
-
- commandProcessorthread.start(commandProcessor);
-
+ // Start all threads
+ commsIn.start(commandProcessor);
commsOut.start(commsOutFunc);
-
motorCtrlT.start(motorCtrlFn);
bitcointhread.start(bitcoin);
- putMessage(WELCOME);
- int8_t orState = motorHome();
- putMessage(ROTOR_ORG);
- //pc.printf("Rotor origin: %x\n\r", orState);
- //Set PWM duty in %
+ // Define motor ISRs
I1.rise(&motorISR);
I2.rise(&motorISR);
I3.rise(&motorISR);