Jacob Kay / Mbed 2 deprecated EMBEDDED_CW2_Final

Final

Dependencies:   Crypto_light mbed-rtos mbed regex

Fork of EMBEDDED_CW2 by George Padley

main.cpp@4:e322ca760c63, 2018-03-20 (annotated)

Committer:
GPadley
Date:
Tue Mar 20 13:23:18 2018 +0000
Revision:
4:e322ca760c63
Parent:
3:569b35e2a602
Child:
5:e4b799086bc1
Cool beans;

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