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@5:e4b799086bc1, 2018-03-23 (annotated)

Committer:
JacobKay97
Date:
Fri Mar 23 15:42:42 2018 +0000
Revision:
5:e4b799086bc1
Parent:
4:e322ca760c63 
Final

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