George Padley / Mbed 2 deprecated EMBEDDED_CW2_Final

Final version

Dependencies:   Crypto_light mbed-rtos mbed

Fork of EMBEDDED_CW2_Final by Jacob Kay

main.cpp@6:96383d87c51f, 2018-03-23 (annotated)

Committer:
GPadley
Date:
Fri Mar 23 21:01:44 2018 +0000
Revision:
6:96383d87c51f
Parent:
5:e4b799086bc1 
Final Commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
estott 0:de4320f74764 1 #include "mbed.h"
GPadley 4:e322ca760c63 2 #include "SHA256.h"
GPadley 4:e322ca760c63 3 #include "rtos.h"
GPadley 4:e322ca760c63 4 #define char_len_max 32
estott 0:de4320f74764 5
estott 0:de4320f74764 6 //Photointerrupter input pins
estott 0:de4320f74764 7 #define I1pin D2
estott 2:4e88faab6988 8 #define I2pin D11
estott 2:4e88faab6988 9 #define I3pin D12
estott 2:4e88faab6988 10
estott 2:4e88faab6988 11 //Incremental encoder input pins
estott 2:4e88faab6988 12 #define CHA D7
GPadley 4:e322ca760c63 13 #define CHB D8
estott 0:de4320f74764 14
estott 0:de4320f74764 15 //Motor Drive output pins //Mask in output byte
estott 2:4e88faab6988 16 #define L1Lpin D4 //0x01
estott 2:4e88faab6988 17 #define L1Hpin D5 //0x02
estott 2:4e88faab6988 18 #define L2Lpin D3 //0x04
estott 2:4e88faab6988 19 #define L2Hpin D6 //0x08
estott 2:4e88faab6988 20 #define L3Lpin D9 //0x10
estott 0:de4320f74764 21 #define L3Hpin D10 //0x20
estott 0:de4320f74764 22
GPadley 6:96383d87c51f 23
estott 0:de4320f74764 24 //Mapping from sequential drive states to motor phase outputs
estott 0:de4320f74764 25 /*
estott 0:de4320f74764 26 State L1 L2 L3
estott 0:de4320f74764 27 0 H - L
estott 0:de4320f74764 28 1 - H L
estott 0:de4320f74764 29 2 L H -
estott 0:de4320f74764 30 3 L - H
estott 0:de4320f74764 31 4 - L H
estott 0:de4320f74764 32 5 H L -
estott 0:de4320f74764 33 6 - - -
estott 0:de4320f74764 34 7 - - -
estott 0:de4320f74764 35 */
estott 0:de4320f74764 36 //Drive state to output table
estott 0:de4320f74764 37 const int8_t driveTable[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00};
estott 2:4e88faab6988 38
estott 0:de4320f74764 39 //Mapping from interrupter inputs to sequential rotor states. 0x00 and 0x07 are not valid
GPadley 4:e322ca760c63 40 const int8_t stateMap[] = {0x07,0x05,0x03,0x04,0x01,0x00,0x02,0x07};
estott 2:4e88faab6988 41
estott 2:4e88faab6988 42 //Phase lead to make motor spin
GPadley 4:e322ca760c63 43 int8_t lead = 2; //2 for forwards, -2 for backwards
GPadley 4:e322ca760c63 44 int8_t oldLead = lead;
GPadley 4:e322ca760c63 45 //Status LED
estott 0:de4320f74764 46
estott 0:de4320f74764 47 DigitalOut led1(LED1);
estott 0:de4320f74764 48
estott 0:de4320f74764 49 //Photointerrupter inputs
GPadley 4:e322ca760c63 50 InterruptIn I1(I1pin);
GPadley 4:e322ca760c63 51 InterruptIn I2(I2pin);
GPadley 4:e322ca760c63 52 InterruptIn I3(I3pin);
estott 0:de4320f74764 53
estott 0:de4320f74764 54 //Motor Drive outputs
GPadley 4:e322ca760c63 55 PwmOut L1L(L1Lpin);
estott 0:de4320f74764 56 DigitalOut L1H(L1Hpin);
GPadley 4:e322ca760c63 57 PwmOut L2L(L2Lpin);
estott 0:de4320f74764 58 DigitalOut L2H(L2Hpin);
GPadley 4:e322ca760c63 59 PwmOut L3L(L3Lpin);
estott 0:de4320f74764 60 DigitalOut L3H(L3Hpin);
estott 0:de4320f74764 61
GPadley 4:e322ca760c63 62 //Initialise the serial port
GPadley 4:e322ca760c63 63 RawSerial pc(SERIAL_TX, SERIAL_RX);
GPadley 4:e322ca760c63 64
GPadley 4:e322ca760c63 65 //***********Initialisation Our Variables************//
GPadley 4:e322ca760c63 66
GPadley 4:e322ca760c63 67 //Message IDs
GPadley 4:e322ca760c63 68 enum message_code {
GPadley 4:e322ca760c63 69 ERROR_C = 0, //Error message ID
GPadley 4:e322ca760c63 70 HASH = 1, //Hash frequency ID
GPadley 4:e322ca760c63 71 NONCE = 2, //correct nonce ID
GPadley 4:e322ca760c63 72 POSITION = 3, //Starting Rotor ID
GPadley 4:e322ca760c63 73 DECODED = 4, //Decoded message ID
GPadley 6:96383d87c51f 74 NEW_KEY = 5, //new key ID
GPadley 6:96383d87c51f 75 OLD_KEY = 6, //old key ID
GPadley 6:96383d87c51f 76 VELOCITY = 7, //velocity
GPadley 4:e322ca760c63 77 NEW_SPEED = 8,
GPadley 6:96383d87c51f 78 NEW_ROTATIONS = 9
GPadley 4:e322ca760c63 79 };
GPadley 4:e322ca760c63 80
GPadley 4:e322ca760c63 81 //message structure
JacobKay97 5:e4b799086bc1 82 typedef struct {
GPadley 4:e322ca760c63 83 uint8_t code; //ID
GPadley 4:e322ca760c63 84 uint64_t data; //Data
JacobKay97 5:e4b799086bc1 85 float dataf; //Fudged it
JacobKay97 5:e4b799086bc1 86 } message_t;
JacobKay97 5:e4b799086bc1 87
JacobKay97 5:e4b799086bc1 88
GPadley 4:e322ca760c63 89
GPadley 4:e322ca760c63 90 Mail<message_t,16> outMessages; //Output message queue
GPadley 4:e322ca760c63 91 Queue<void, 8> inCharQ; //character inputs
GPadley 4:e322ca760c63 92
GPadley 4:e322ca760c63 93 int8_t orState; //starting state of the rotor
GPadley 4:e322ca760c63 94 uint8_t sequence[] = {0x45,0x6D,0x62,0x65,0x64,0x64,0x65,0x64,0x20,0x53,0x79,0x73,0x74,0x65,0x6D,0x73,0x20,0x61,0x72,0x65,0x20,0x66,0x75,0x6E,0x20,0x61,0x6E,0x64,0x20,0x64,0x6F,0x20,0x61,0x77,0x65,0x73,0x6F,0x6D,0x65,0x20,0x74,0x68,0x69,0x6E,0x67,0x73,0x21,0x20,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
GPadley 4:e322ca760c63 95 uint64_t* key = (uint64_t*)((int)sequence + 48); //Key generation
GPadley 4:e322ca760c63 96 uint64_t* nonce = (uint64_t*)((int)sequence + 56); //Nonce
GPadley 4:e322ca760c63 97 uint8_t hash[32]; //Hash output
GPadley 4:e322ca760c63 98 char commInChar[char_len_max]; //array 32 characters length
GPadley 4:e322ca760c63 99 uint8_t ptr; //char array pointer
GPadley 4:e322ca760c63 100 volatile uint64_t newKey; //means value can change between thread calls
GPadley 4:e322ca760c63 101 uint64_t oldKey;
GPadley 4:e322ca760c63 102 Mutex newKey_mutex; //Stops the value from beng changed during use
JacobKay97 5:e4b799086bc1 103 float newSpeed = 30.0f;
GPadley 4:e322ca760c63 104 Mutex newSpeed_mutex;
GPadley 4:e322ca760c63 105 uint32_t torqueVal = 1000;
JacobKay97 5:e4b799086bc1 106 float noRotations = 0.0f;
GPadley 4:e322ca760c63 107 bool dirSwitch = false;
GPadley 4:e322ca760c63 108 bool rotate = false;
GPadley 4:e322ca760c63 109 bool rotStart = false;
GPadley 6:96383d87c51f 110 int period = 2000;
GPadley 6:96383d87c51f 111 int kp = 25.0f;
GPadley 6:96383d87c51f 112 int kd = 20.0f;
GPadley 4:e322ca760c63 113
GPadley 4:e322ca760c63 114 Thread commOutT(osPriorityNormal,1024); //Output Thread
JacobKay97 5:e4b799086bc1 115 Thread commInT(osPriorityNormal,1200); //Input Thread
GPadley 4:e322ca760c63 116 Thread motorCtrlT(osPriorityNormal,1024);
GPadley 4:e322ca760c63 117
JacobKay97 5:e4b799086bc1 118 void init_pwm()
JacobKay97 5:e4b799086bc1 119 {
GPadley 4:e322ca760c63 120 L1L.period_us(period);
GPadley 4:e322ca760c63 121 L2L.period_us(period);
GPadley 4:e322ca760c63 122 L3L.period_us(period);
GPadley 4:e322ca760c63 123 }
GPadley 4:e322ca760c63 124
GPadley 4:e322ca760c63 125
JacobKay97 5:e4b799086bc1 126 void putMessage(uint8_t code, uint64_t data)
JacobKay97 5:e4b799086bc1 127 {
GPadley 4:e322ca760c63 128 message_t *pMessage = outMessages.alloc(); //allocated the recieved message to outmessages
GPadley 4:e322ca760c63 129 pMessage->code = code;
GPadley 4:e322ca760c63 130 pMessage->data = data;
GPadley 4:e322ca760c63 131 outMessages.put(pMessage);
JacobKay97 5:e4b799086bc1 132 }
estott 0:de4320f74764 133
JacobKay97 5:e4b799086bc1 134 void putMessage(uint8_t code, float data)
JacobKay97 5:e4b799086bc1 135 {
JacobKay97 5:e4b799086bc1 136 message_t *pMessage = outMessages.alloc(); //allocated the recieved message to outmessages
JacobKay97 5:e4b799086bc1 137 pMessage->code = code;
JacobKay97 5:e4b799086bc1 138 pMessage->dataf = data;
JacobKay97 5:e4b799086bc1 139 outMessages.put(pMessage);
JacobKay97 5:e4b799086bc1 140 }
JacobKay97 5:e4b799086bc1 141
JacobKay97 5:e4b799086bc1 142 void commOutFn()
JacobKay97 5:e4b799086bc1 143 {
JacobKay97 5:e4b799086bc1 144 while(1) {
GPadley 4:e322ca760c63 145 osEvent newEvent = outMessages.get(); //pulls the message
GPadley 4:e322ca760c63 146 message_t *pMessage = (message_t*)newEvent.value.p; //assigns the values to pmessage
GPadley 4:e322ca760c63 147
JacobKay97 5:e4b799086bc1 148 switch(pMessage->code) { //finds correct ID for message
GPadley 4:e322ca760c63 149 case ERROR_C:
JacobKay97 5:e4b799086bc1 150 if(pMessage->data == 0) { //Input message was too large
GPadley 4:e322ca760c63 151 pc.printf("Input command too large\n\r");
GPadley 4:e322ca760c63 152 }
GPadley 4:e322ca760c63 153 break;
GPadley 4:e322ca760c63 154 case HASH:
JacobKay97 5:e4b799086bc1 155 pc.printf("Hash Rate %d Hashes/sec \n\r",pMessage->data); //outputs the hash frequency
GPadley 4:e322ca760c63 156 break;
GPadley 4:e322ca760c63 157 case NONCE:
GPadley 4:e322ca760c63 158 pc.printf("Found a nonce 0x%016x\n\r", pMessage->data); //outputs correct nonce
GPadley 4:e322ca760c63 159 break;
GPadley 4:e322ca760c63 160 case POSITION:
GPadley 4:e322ca760c63 161 pc.printf("Rotor Starting Position: %d\n\r", pMessage->data); //outputs starting position
GPadley 4:e322ca760c63 162 break;
GPadley 4:e322ca760c63 163 case DECODED:
GPadley 4:e322ca760c63 164 if (pMessage->data == 0) {
GPadley 4:e322ca760c63 165 pc.printf("Decoded as max speed\n\r");
JacobKay97 5:e4b799086bc1 166 } else if (pMessage->data == 1) {
GPadley 4:e322ca760c63 167 pc.printf("Decoded no rotations\n\r");
JacobKay97 5:e4b799086bc1 168 } else if (pMessage->data == 2) {
GPadley 4:e322ca760c63 169 pc.printf("Decoded key K\n\r");
JacobKay97 5:e4b799086bc1 170 } else if (pMessage->data == 3) {
GPadley 4:e322ca760c63 171 pc.printf("Decoded torque T\n\r");
GPadley 4:e322ca760c63 172 }
GPadley 4:e322ca760c63 173 break;
GPadley 4:e322ca760c63 174 case NEW_KEY:
GPadley 4:e322ca760c63 175 pc.printf("Decoded new key 0x%016llx\n\r",pMessage->data);
GPadley 4:e322ca760c63 176 break;
GPadley 4:e322ca760c63 177 case OLD_KEY:
GPadley 4:e322ca760c63 178 pc.printf("Decoded new key same as old key: 0x%016llx\n\r",pMessage->data);
GPadley 4:e322ca760c63 179 break;
GPadley 4:e322ca760c63 180 case VELOCITY:
JacobKay97 5:e4b799086bc1 181 pc.printf("Current speed: %f\n\r",pMessage->dataf);
GPadley 4:e322ca760c63 182 break;
GPadley 4:e322ca760c63 183 case NEW_SPEED:
JacobKay97 5:e4b799086bc1 184 pc.printf("New speed: %f\n\r",pMessage->dataf);
GPadley 4:e322ca760c63 185 break;
GPadley 4:e322ca760c63 186 case NEW_ROTATIONS:
JacobKay97 5:e4b799086bc1 187 pc.printf("New number of rotations: %f\n\r",pMessage->dataf);
GPadley 4:e322ca760c63 188 break;
GPadley 4:e322ca760c63 189 }
GPadley 4:e322ca760c63 190 outMessages.free(pMessage); //removes the message
GPadley 4:e322ca760c63 191 }
estott 0:de4320f74764 192 }
GPadley 4:e322ca760c63 193
JacobKay97 5:e4b799086bc1 194 void serialISR()
JacobKay97 5:e4b799086bc1 195 {
GPadley 4:e322ca760c63 196 uint8_t newChar = pc.getc(); //gets valuee from serial port
GPadley 4:e322ca760c63 197 inCharQ.put((void*)newChar); //places into newChar
JacobKay97 5:e4b799086bc1 198 }
GPadley 4:e322ca760c63 199
JacobKay97 5:e4b799086bc1 200 void decode_char(char* buffer, uint8_t index)
JacobKay97 5:e4b799086bc1 201 {
JacobKay97 5:e4b799086bc1 202 if(buffer[index] == 'V') { //if first value is R rotate cretain number of times
JacobKay97 5:e4b799086bc1 203 putMessage(DECODED,(uint64_t)0);
GPadley 4:e322ca760c63 204 newSpeed_mutex.lock();
JacobKay97 5:e4b799086bc1 205 sscanf(buffer, "V%f", &newSpeed);
JacobKay97 5:e4b799086bc1 206 if(newSpeed == 0.0f) {
JacobKay97 5:e4b799086bc1 207 newSpeed = 120.0f;
JacobKay97 5:e4b799086bc1 208 } else if(newSpeed < 0.0f) {
JacobKay97 5:e4b799086bc1 209 newSpeed = fabsf(newSpeed);
GPadley 4:e322ca760c63 210 }
GPadley 4:e322ca760c63 211 putMessage(NEW_SPEED,newSpeed);
GPadley 4:e322ca760c63 212 newSpeed_mutex.unlock();
JacobKay97 5:e4b799086bc1 213 } else if(buffer[index] == 'v') { //if first value is R rotate cretain number of times
JacobKay97 5:e4b799086bc1 214 putMessage(DECODED,(uint64_t)0);
GPadley 4:e322ca760c63 215 newSpeed_mutex.lock();
JacobKay97 5:e4b799086bc1 216 sscanf(buffer, "v%f", &newSpeed);
JacobKay97 5:e4b799086bc1 217 if(newSpeed == 0.0f) {
JacobKay97 5:e4b799086bc1 218 newSpeed = 120.0f;
JacobKay97 5:e4b799086bc1 219 } else if(newSpeed < 0.0f) {
JacobKay97 5:e4b799086bc1 220 newSpeed = fabsf(newSpeed);
GPadley 4:e322ca760c63 221 }
GPadley 4:e322ca760c63 222 putMessage(NEW_SPEED,newSpeed);
GPadley 4:e322ca760c63 223 newSpeed_mutex.unlock();
GPadley 4:e322ca760c63 224
JacobKay97 5:e4b799086bc1 225 } else if(buffer[index] == 'R') { //if first value is V set speed of rotation
JacobKay97 5:e4b799086bc1 226 putMessage(DECODED,(uint64_t)1);
JacobKay97 5:e4b799086bc1 227 sscanf(buffer, "R%f", &noRotations);
GPadley 4:e322ca760c63 228 rotate = true;
GPadley 4:e322ca760c63 229 rotStart = true;
GPadley 4:e322ca760c63 230 putMessage(NEW_ROTATIONS,noRotations);
JacobKay97 5:e4b799086bc1 231 } else if(buffer[index] == 'r') { //if first value is V set speed of rotation
JacobKay97 5:e4b799086bc1 232 putMessage(DECODED,(uint64_t)1);
JacobKay97 5:e4b799086bc1 233 sscanf(buffer, "r%f", &noRotations);
GPadley 4:e322ca760c63 234 rotate = true;
GPadley 4:e322ca760c63 235 rotStart = true;
GPadley 4:e322ca760c63 236 putMessage(NEW_ROTATIONS,noRotations);
JacobKay97 5:e4b799086bc1 237 } else if (buffer[index] == 'K') { //if char is K set key to value input
GPadley 6:96383d87c51f 238 newKey_mutex.lock();
GPadley 6:96383d87c51f 239 sscanf(buffer, "K%llx", &newKey);
GPadley 6:96383d87c51f 240 if(oldKey != newKey) {
GPadley 6:96383d87c51f 241 putMessage(NEW_KEY,newKey);
GPadley 6:96383d87c51f 242 *key = newKey;
GPadley 6:96383d87c51f 243 oldKey = newKey;
JacobKay97 5:e4b799086bc1 244 } else {
GPadley 6:96383d87c51f 245 putMessage(OLD_KEY,oldKey);
GPadley 4:e322ca760c63 246 }
GPadley 6:96383d87c51f 247 newKey_mutex.unlock();
JacobKay97 5:e4b799086bc1 248 } else if (buffer[index] == 'k') { //if char is K set key to value input
JacobKay97 5:e4b799086bc1 249 putMessage(DECODED,(uint64_t)2);
JacobKay97 5:e4b799086bc1 250 newKey_mutex.lock();
JacobKay97 5:e4b799086bc1 251 sscanf(buffer, "k%llx", &newKey);
JacobKay97 5:e4b799086bc1 252 if(oldKey != newKey) {
JacobKay97 5:e4b799086bc1 253 putMessage(NEW_KEY,newKey);
JacobKay97 5:e4b799086bc1 254 *key = newKey;
JacobKay97 5:e4b799086bc1 255 oldKey = newKey;
JacobKay97 5:e4b799086bc1 256 } else {
JacobKay97 5:e4b799086bc1 257 putMessage(OLD_KEY,oldKey);
JacobKay97 5:e4b799086bc1 258 }
JacobKay97 5:e4b799086bc1 259 newKey_mutex.unlock();
GPadley 4:e322ca760c63 260 }
GPadley 4:e322ca760c63 261 }
GPadley 4:e322ca760c63 262
JacobKay97 5:e4b799086bc1 263 void commInFn()
JacobKay97 5:e4b799086bc1 264 {
GPadley 4:e322ca760c63 265 pc.printf("Enter your command:\n\r"); //Tells the person to input their message
GPadley 4:e322ca760c63 266 pc.attach(&serialISR); //looks for the serialISR to get message
JacobKay97 5:e4b799086bc1 267 while(1) {
GPadley 6:96383d87c51f 268
JacobKay97 5:e4b799086bc1 269 if(ptr >= char_len_max) {
JacobKay97 5:e4b799086bc1 270 putMessage(ERROR_C,(uint64_t)0); //if gone over the buffer length, cancel and restart for next input
GPadley 4:e322ca760c63 271 ptr = 0; //reset pointer
GPadley 4:e322ca760c63 272 }
GPadley 4:e322ca760c63 273 osEvent newEvent = inCharQ.get(); //get next character
GPadley 4:e322ca760c63 274 uint8_t newChar = (uint8_t)newEvent.value.p;
JacobKay97 5:e4b799086bc1 275 if(newChar != '\r' && newChar != '\n') {
GPadley 4:e322ca760c63 276 commInChar[ptr] = newChar; //place values into buffer
GPadley 4:e322ca760c63 277 ptr++; //increment pointer
JacobKay97 5:e4b799086bc1 278 } else {
JacobKay97 5:e4b799086bc1 279 // commInChar[ptr] = '\0'; //defines the end of the command
JacobKay97 5:e4b799086bc1 280 commInChar[ptr] = ' '; //defines the end of the command
GPadley 4:e322ca760c63 281 ptr = 0; //resets the pointer
GPadley 4:e322ca760c63 282 decode_char(commInChar,ptr); //sends array to decoding function
estott 0:de4320f74764 283 }
GPadley 6:96383d87c51f 284
estott 2:4e88faab6988 285 }
estott 0:de4320f74764 286 }
estott 0:de4320f74764 287
GPadley 4:e322ca760c63 288 //Set a given drive state
JacobKay97 5:e4b799086bc1 289 void motorOut(int8_t driveState, uint32_t torque)
JacobKay97 5:e4b799086bc1 290 {
GPadley 4:e322ca760c63 291
GPadley 4:e322ca760c63 292 //Lookup the output byte from the drive state.
GPadley 4:e322ca760c63 293 int8_t driveOut = driveTable[driveState & 0x07];
GPadley 4:e322ca760c63 294
GPadley 4:e322ca760c63 295 //Turn off first
GPadley 4:e322ca760c63 296 if (~driveOut & 0x01) L1L.pulsewidth_us(0);
GPadley 4:e322ca760c63 297 if (~driveOut & 0x02) L1H = 1;
GPadley 4:e322ca760c63 298 if (~driveOut & 0x04) L2L.pulsewidth_us(0);
GPadley 4:e322ca760c63 299 if (~driveOut & 0x08) L2H = 1;
GPadley 4:e322ca760c63 300 if (~driveOut & 0x10) L3L.pulsewidth_us(0);
GPadley 4:e322ca760c63 301 if (~driveOut & 0x20) L3H = 1;
GPadley 4:e322ca760c63 302
GPadley 4:e322ca760c63 303 //Then turn on
GPadley 6:96383d87c51f 304 if (driveOut & 0x01) L1L.pulsewidth_us(torque); //motor torque output
GPadley 4:e322ca760c63 305 if (driveOut & 0x02) L1H = 0;
GPadley 4:e322ca760c63 306 if (driveOut & 0x04) L2L.pulsewidth_us(torque);
GPadley 4:e322ca760c63 307 if (driveOut & 0x08) L2H = 0;
GPadley 4:e322ca760c63 308 if (driveOut & 0x10) L3L.pulsewidth_us(torque);
GPadley 4:e322ca760c63 309 if (driveOut & 0x20) L3H = 0;
GPadley 4:e322ca760c63 310 }
GPadley 4:e322ca760c63 311
GPadley 4:e322ca760c63 312 //Convert photointerrupter inputs to a rotor state
JacobKay97 5:e4b799086bc1 313 inline int8_t readRotorState()
JacobKay97 5:e4b799086bc1 314 {
GPadley 4:e322ca760c63 315 return stateMap[I1 + 2*I2 + 4*I3];
GPadley 4:e322ca760c63 316 }
GPadley 4:e322ca760c63 317
GPadley 4:e322ca760c63 318 //Basic synchronisation routine
JacobKay97 5:e4b799086bc1 319 int8_t motorHome()
JacobKay97 5:e4b799086bc1 320 {
GPadley 4:e322ca760c63 321 //Put the motor in drive state 0 and wait for it to stabilise
JacobKay97 5:e4b799086bc1 322 //motorOut(0,torqueVal);
JacobKay97 5:e4b799086bc1 323 motorOut(0,1000);
GPadley 6:96383d87c51f 324 wait(1.0); //waits for stabalisation
GPadley 6:96383d87c51f 325 lead = 0; //stops rotation
GPadley 4:e322ca760c63 326
GPadley 4:e322ca760c63 327 //Get the rotor state
GPadley 4:e322ca760c63 328 return readRotorState();
GPadley 4:e322ca760c63 329 }
GPadley 4:e322ca760c63 330
GPadley 4:e322ca760c63 331 int32_t motorPosition;
JacobKay97 5:e4b799086bc1 332 void motorISR()
JacobKay97 5:e4b799086bc1 333 {
GPadley 6:96383d87c51f 334 static int8_t oldRotorState; //remembers old state
GPadley 4:e322ca760c63 335 int8_t rotorState = readRotorState(); //reads motor position
GPadley 4:e322ca760c63 336 motorOut((rotorState-orState+lead+6)%6,torqueVal); //+6 to make sure the remainder is positive
GPadley 6:96383d87c51f 337 if(rotorState - oldRotorState==5) motorPosition--; //reverse
GPadley 6:96383d87c51f 338 else if(rotorState - oldRotorState== -5) motorPosition++; //forward
JacobKay97 5:e4b799086bc1 339 else motorPosition+=(rotorState - oldRotorState);
GPadley 6:96383d87c51f 340 oldRotorState = rotorState;//remember previous state
GPadley 4:e322ca760c63 341 }
GPadley 4:e322ca760c63 342
JacobKay97 5:e4b799086bc1 343 void motorCtrlTick()
JacobKay97 5:e4b799086bc1 344 {
GPadley 6:96383d87c51f 345 motorCtrlT.signal_set(0x1); //wait for 100 ms to send signal
GPadley 4:e322ca760c63 346 }
GPadley 4:e322ca760c63 347
GPadley 4:e322ca760c63 348 Timer t_motor;
JacobKay97 5:e4b799086bc1 349 void motorCtrlFn()
JacobKay97 5:e4b799086bc1 350 {
GPadley 4:e322ca760c63 351
GPadley 6:96383d87c51f 352 float v, v_avg, ys, yr, dEr; //local variables used
GPadley 4:e322ca760c63 353 int i = 0, dt, oldPosition, totPosition, position, startPosition, newEr, oldEr, mainLead;
GPadley 6:96383d87c51f 354 bool jumpStart = false;
GPadley 4:e322ca760c63 355 t_motor.start();
GPadley 4:e322ca760c63 356 Ticker motorCtrlTicker;
GPadley 4:e322ca760c63 357 motorCtrlTicker.attach_us(&motorCtrlTick,100000);
JacobKay97 5:e4b799086bc1 358 while(1) {
JacobKay97 5:e4b799086bc1 359 motorCtrlT.signal_wait(0x1);
JacobKay97 5:e4b799086bc1 360 if(rotate) {
JacobKay97 5:e4b799086bc1 361 if(rotStart) {
JacobKay97 5:e4b799086bc1 362 if(noRotations > 0) {
GPadley 6:96383d87c51f 363 lead = 2;//positive rotations
JacobKay97 5:e4b799086bc1 364 } else if(noRotations < 0) {
GPadley 6:96383d87c51f 365 lead = -2;//negative rotations
JacobKay97 5:e4b799086bc1 366 } else if(noRotations == 0 && lead == 0) {
GPadley 6:96383d87c51f 367 lead = 2;//if no direction specified spin forwards
GPadley 4:e322ca760c63 368 }
GPadley 6:96383d87c51f 369 i = 0; //reset variables
GPadley 4:e322ca760c63 370 v_avg = 0;
JacobKay97 5:e4b799086bc1 371 yr = 0.0f;
JacobKay97 5:e4b799086bc1 372 ys = 0.0f;
JacobKay97 5:e4b799086bc1 373 dEr = 0.0f;
GPadley 4:e322ca760c63 374 mainLead = lead; //sets general direction
GPadley 6:96383d87c51f 375 totPosition = (int)6*noRotations; //number of position changes required
GPadley 4:e322ca760c63 376 oldEr = totPosition; //how far away
GPadley 4:e322ca760c63 377 rotStart = false; //stops from running this loop
GPadley 4:e322ca760c63 378 __disable_irq(); //disables interrupts
GPadley 4:e322ca760c63 379 startPosition = motorPosition; //sets start position at present motor position
GPadley 4:e322ca760c63 380 oldPosition = startPosition; //sets old position to same value
GPadley 4:e322ca760c63 381 t_motor.reset();
GPadley 4:e322ca760c63 382 motorPosition = 0; //resets time and motorPosition
GPadley 4:e322ca760c63 383 __enable_irq(); //enables interrupts
JacobKay97 5:e4b799086bc1 384 position = 0;
GPadley 6:96383d87c51f 385 jumpStart = true;
JacobKay97 5:e4b799086bc1 386
JacobKay97 5:e4b799086bc1 387 } else if(noRotations == 0) { //if to spin forever
GPadley 4:e322ca760c63 388 i++; //increment counter
GPadley 4:e322ca760c63 389 __disable_irq();
GPadley 4:e322ca760c63 390 position = motorPosition; //adds on number of rotations
GPadley 4:e322ca760c63 391 dt = t_motor.read_ms(); //change in time
GPadley 4:e322ca760c63 392 t_motor.reset(); //resets time
GPadley 4:e322ca760c63 393 motorPosition = 0; //resets motor position
GPadley 4:e322ca760c63 394 __enable_irq(); //enables interrupts
JacobKay97 5:e4b799086bc1 395 v = (166.67f*((float)position/(float)dt)); //calculates velocity
GPadley 4:e322ca760c63 396 v_avg += v; //adds speed onto averager
JacobKay97 5:e4b799086bc1 397 newSpeed_mutex.lock();
JacobKay97 5:e4b799086bc1 398 if((int)abs(v) < 1 && newSpeed != 0) {
GPadley 4:e322ca760c63 399 lead = mainLead; //makes sure it's in the correct direction
GPadley 4:e322ca760c63 400 torqueVal = 1000; //sets torque
GPadley 4:e322ca760c63 401 motorISR(); //moves the motor
GPadley 4:e322ca760c63 402 }
JacobKay97 5:e4b799086bc1 403 newSpeed_mutex.unlock();
JacobKay97 5:e4b799086bc1 404 ys = kp*(newSpeed-abs(v)); //speed controller
JacobKay97 5:e4b799086bc1 405 if(ys < 0) {
GPadley 6:96383d87c51f 406 lead = mainLead*-1; //reverses direction
JacobKay97 5:e4b799086bc1 407 } else {
GPadley 6:96383d87c51f 408 lead = mainLead; //goes forwards
JacobKay97 5:e4b799086bc1 409 }
JacobKay97 5:e4b799086bc1 410 if(abs(ys) > 1000) {
GPadley 6:96383d87c51f 411 torqueVal = 1000; //maximum speed
JacobKay97 5:e4b799086bc1 412 } else {
GPadley 6:96383d87c51f 413 torqueVal = abs(ys); //makes sure absolute
JacobKay97 5:e4b799086bc1 414 }
JacobKay97 5:e4b799086bc1 415 // pc.printf("torque = %d\r\n",torqueVal);
JacobKay97 5:e4b799086bc1 416 } else {
JacobKay97 5:e4b799086bc1 417 i++; //increment counter
JacobKay97 5:e4b799086bc1 418 __disable_irq();
JacobKay97 5:e4b799086bc1 419 position += motorPosition; //adds on number of rotations
JacobKay97 5:e4b799086bc1 420 dt = t_motor.read_ms(); //change in time
JacobKay97 5:e4b799086bc1 421 // pc.printf("motorPosPre = %d\r\n",motorPosition);
JacobKay97 5:e4b799086bc1 422 t_motor.reset(); //resets time
JacobKay97 5:e4b799086bc1 423 motorPosition = 0; //resets motor position
JacobKay97 5:e4b799086bc1 424 __enable_irq(); //enables interrupts
JacobKay97 5:e4b799086bc1 425 // pc.printf("Pos = %d\r\n",position);
JacobKay97 5:e4b799086bc1 426 v = 166.67f*(((float)position-(float)oldPosition)/(float)dt); //calculates velocity
JacobKay97 5:e4b799086bc1 427
JacobKay97 5:e4b799086bc1 428 oldPosition = position; //changes old position
JacobKay97 5:e4b799086bc1 429 newEr = totPosition-position; //difference in placement
JacobKay97 5:e4b799086bc1 430 dEr = 1000.0f*((float)newEr-(float)oldEr)/(float)dt; //change against time
JacobKay97 5:e4b799086bc1 431 oldEr = newEr; //old is same as new
GPadley 6:96383d87c51f 432 yr = (float)kp*(float)newEr + (float)kd*dEr; //rotational controller
JacobKay97 5:e4b799086bc1 433 v_avg += v; //adds speed onto averager
JacobKay97 5:e4b799086bc1 434 ys = (float)kp*((float)newSpeed-fabsf(v))*((newEr > 0) ? 1.0f : ((newEr < 0) ? -1.0f : 0.0f)); //speed controller
GPadley 6:96383d87c51f 435 if(jumpStart == true) {
JacobKay97 5:e4b799086bc1 436 lead = mainLead; //makes sure it's in the correct direction
JacobKay97 5:e4b799086bc1 437 torqueVal = 900; //sets torque
JacobKay97 5:e4b799086bc1 438 motorISR(); //moves the motor
GPadley 6:96383d87c51f 439 jumpStart = false;
JacobKay97 5:e4b799086bc1 440
JacobKay97 5:e4b799086bc1 441 }
JacobKay97 5:e4b799086bc1 442 if(v >=0.0f) { //if speed is +ve
JacobKay97 5:e4b799086bc1 443 if(abs(ys)<abs(yr)) {
GPadley 6:96383d87c51f 444 torqueVal = abs(ys); //sets the torque output
JacobKay97 5:e4b799086bc1 445 } else {
GPadley 6:96383d87c51f 446 torqueVal = abs(yr);//sets the torque output
JacobKay97 5:e4b799086bc1 447 }
GPadley 6:96383d87c51f 448 if(abs(newEr) <=5) {//stops it spinning
JacobKay97 5:e4b799086bc1 449 torqueVal = 0;
JacobKay97 5:e4b799086bc1 450 lead = 0;
JacobKay97 5:e4b799086bc1 451 rotate = false;
JacobKay97 5:e4b799086bc1 452 pc.printf("NewErr %d\r\n",newEr);
GPadley 6:96383d87c51f 453 } else if (yr<-500 && dEr <0) {
GPadley 6:96383d87c51f 454 torqueVal = abs(yr);//go backwards
JacobKay97 5:e4b799086bc1 455 lead = -2;
JacobKay97 5:e4b799086bc1 456 } else {
GPadley 6:96383d87c51f 457 lead = 2; //go forwards
GPadley 4:e322ca760c63 458 }
JacobKay97 5:e4b799086bc1 459 } else {
JacobKay97 5:e4b799086bc1 460 if(abs(ys)>abs(yr)) {
GPadley 6:96383d87c51f 461 torqueVal = abs(ys); //take the highest value for torque
JacobKay97 5:e4b799086bc1 462 } else {
JacobKay97 5:e4b799086bc1 463 torqueVal = abs(yr);
JacobKay97 5:e4b799086bc1 464 }
GPadley 6:96383d87c51f 465 if(abs(newEr) <=5) { //stop spinning when error loess than 1 rotation
JacobKay97 5:e4b799086bc1 466 torqueVal = 0;
JacobKay97 5:e4b799086bc1 467 lead = 0;
JacobKay97 5:e4b799086bc1 468 rotate = false;
GPadley 6:96383d87c51f 469 } else if (yr>500 && dEr >0) { //goes backwards if overshot
JacobKay97 5:e4b799086bc1 470 torqueVal = abs(yr);
JacobKay97 5:e4b799086bc1 471 lead = 2;
JacobKay97 5:e4b799086bc1 472 } else {
JacobKay97 5:e4b799086bc1 473 lead = -2;
GPadley 4:e322ca760c63 474 }
GPadley 4:e322ca760c63 475 }
GPadley 6:96383d87c51f 476 if(torqueVal != 0 && lead !=0 && abs(v)== 0) { //makes sure motor spins if it is meant to
JacobKay97 5:e4b799086bc1 477 torqueVal = torqueVal + 50;
JacobKay97 5:e4b799086bc1 478 motorISR();
JacobKay97 5:e4b799086bc1 479 }
GPadley 6:96383d87c51f 480 if(torqueVal > 1000) { //stops non linear torque
JacobKay97 5:e4b799086bc1 481 torqueVal = 1000;
GPadley 4:e322ca760c63 482 }
GPadley 4:e322ca760c63 483 }
JacobKay97 5:e4b799086bc1 484 }
GPadley 6:96383d87c51f 485 if (i==10) {//every 1 second print velocity
JacobKay97 5:e4b799086bc1 486 v_avg = v_avg/i;
JacobKay97 5:e4b799086bc1 487 putMessage(VELOCITY, v_avg);
JacobKay97 5:e4b799086bc1 488 v_avg = 0;
JacobKay97 5:e4b799086bc1 489 i= 0;
GPadley 4:e322ca760c63 490 }
GPadley 4:e322ca760c63 491 }
GPadley 4:e322ca760c63 492 }
JacobKay97 5:e4b799086bc1 493
JacobKay97 5:e4b799086bc1 494
GPadley 4:e322ca760c63 495 //Main
JacobKay97 5:e4b799086bc1 496 int main()
JacobKay97 5:e4b799086bc1 497 {
GPadley 4:e322ca760c63 498 pc.printf("Hello\n\r"); //outputs hello when turned on
GPadley 4:e322ca760c63 499 init_pwm();
GPadley 4:e322ca760c63 500 commOutT.start(commOutFn); //starts the output and input threads
GPadley 4:e322ca760c63 501 commInT.start(commInFn);
GPadley 4:e322ca760c63 502 //Run the motor synchronisation
GPadley 4:e322ca760c63 503 orState = motorHome(); //finds staring position
JacobKay97 5:e4b799086bc1 504 putMessage(POSITION,(uint64_t)orState);
GPadley 4:e322ca760c63 505
GPadley 4:e322ca760c63 506 Timer t; //adds a timer to count number of hashes per second
GPadley 4:e322ca760c63 507 //orState is subtracted from future rotor state inputs to align rotor and motor states
GPadley 4:e322ca760c63 508 //Poll the rotor state and set the motor outputs accordingly to spin the motor
GPadley 4:e322ca760c63 509 I1.rise(&motorISR); //looks for rising edge to trigger the motor change
GPadley 4:e322ca760c63 510 I2.rise(&motorISR);
GPadley 4:e322ca760c63 511 I3.rise(&motorISR);
GPadley 4:e322ca760c63 512 I1.fall(&motorISR); //looks for rising edge to trigger the motor change
GPadley 4:e322ca760c63 513 I2.fall(&motorISR);
GPadley 4:e322ca760c63 514 I3.fall(&motorISR);
GPadley 4:e322ca760c63 515 uint16_t counter;
GPadley 4:e322ca760c63 516 counter = 0; //initialised and set to 0 to count number of hashes
GPadley 4:e322ca760c63 517 t.start(); //starts the timer
GPadley 4:e322ca760c63 518 motorCtrlT.start(motorCtrlFn);
GPadley 4:e322ca760c63 519 while (1) {
GPadley 4:e322ca760c63 520
JacobKay97 5:e4b799086bc1 521 if(t.read_ms() >= 1000) { //if more than 1 second has surpased
JacobKay97 5:e4b799086bc1 522 putMessage(HASH, (uint64_t)counter); //outputs the hash frequency
GPadley 4:e322ca760c63 523 counter = 0; //reset counter
GPadley 4:e322ca760c63 524 t.reset(); //resets the timer
GPadley 4:e322ca760c63 525 }
GPadley 4:e322ca760c63 526 SHA256::computeHash(&hash[0],&sequence[0],sizeof(sequence)); //computes the hash
GPadley 4:e322ca760c63 527 counter++; //increments counter;
GPadley 4:e322ca760c63 528
JacobKay97 5:e4b799086bc1 529 if((hash[0] == 0) && (hash[1] == 0)) {
JacobKay97 5:e4b799086bc1 530 putMessage(NONCE,*nonce); //when hash is correct print the nonce
GPadley 4:e322ca760c63 531 }
GPadley 4:e322ca760c63 532
GPadley 4:e322ca760c63 533 *nonce += 1; //increments nonce
GPadley 4:e322ca760c63 534 }
GPadley 4:e322ca760c63 535 }