Olaf Sikorski
This is probably never gonna get done
Revision 29:cb4db90cfd63, committed 2019-03-22
- Comitter:
- Olaffo
- Date:
- Fri Mar 22 20:28:28 2019 +0000
- Parent:
- 28:613a88b88074
- Child:
- 30:1405ab394f02
- Commit message:
- final version???
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Fri Mar 22 19:13:05 2019 +0000
+++ b/main.cpp Fri Mar 22 20:28:28 2019 +0000
@@ -24,6 +24,10 @@
#define MCSPpin A1
#define MCSNpin A0
+
+//Test pin
+#define Test D4
+
//Mapping from sequential drive states to motor phase outputs
/*
State L1 L2 L3
@@ -48,18 +52,13 @@
//Status LED
DigitalOut led1(LED1);
+DigitalOut TestPin(Test)
//Photointerrupter inputs
InterruptIn I1(I1pin);
InterruptIn I2(I2pin);
InterruptIn I3(I3pin);
-//Fine interrupts
-InterruptIn CHA(CHApin);
-InterruptIn CHB(CHBpin);
-uint8_t attached = 0; //interrupt state tracker
-DigitalOut IDC(Indicator);
-
//Motor Drive outputs
DigitalOut L1L(L1Lpin);
DigitalOut L1H(L1Hpin);
@@ -70,26 +69,21 @@
PwmOut d9 (PWMpin);
int8_t orState = 0;
-//int8_t intState = 0;
-//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
-
typedef struct {
int message;
uint64_t data;
float fdata;
} mail_t;
-Mail<mail_t, 1024> mail_box;
-Thread commsIn(osPriorityNormal,1024);
-Thread bitcointhread(osPriorityNormal,1024);
-Thread motorCtrlT(osPriorityNormal,1024);
-Thread commsOut(osPriorityNormal,1024);
+Mail<mail_t, 16> mail_box;
+Thread commsIn(osPriorityNormal2,1024);
+Thread bitcointhread(osPriorityLow,1024);
+Thread motorCtrlT(osPriorityHigh,1024);
+Thread commsOut(osPriorityNormal1,1024);
RawSerial pc(SERIAL_TX, SERIAL_RX);
Queue<void, 8> inCharQ;
@@ -101,51 +95,11 @@
float pulseWidth;
float motorPosition_command;
float motorPosition;
-//float pulseWidth;
-
-uint32_t revCount,encCount,maxEncCount,minEncCount,badEdges,maxBadEdges;
-uint32_t encState,totalEncCount;
-uint32_t encArray[1024];
-int encIndex = 0;
void serialISR() {
uint8_t newChar = pc.getc();
inCharQ.put((void*) newChar);
}
-
-
-/***************************************************************/
-//Fine control interrupt functions
-
-void encISR0() {
- if (encState == 3) {encCount++;IDC = !IDC;}
- else badEdges++;
- encState = 0;
- }
-
-// B Rise
-void encISR1() {
- if (encState == 0) {encCount++;IDC = !IDC;}
- else badEdges++;
- encState = 1;
- }
-
-// A Fall
-void encISR2() {
- if (encState == 1) {encCount++;IDC = !IDC;}
- else badEdges++;
- encState = 2;
- }
-
-// B Fall
-void encISR3() {
- if (encState == 2) {encCount++;IDC = !IDC;}
- else badEdges++;
- encState = 3;
- }
-//End of block 2
-/***************************************************************/
-
/***************************************************************/
//The following block should be moved to a library, but I don't the time to figure this out atm.
@@ -160,11 +114,6 @@
TUNE_CMD = 5,
STANDARD = 6,
FOREVER_FORWARD = 7,
-LEAD = 9,
-FINE_TUNE = 21,
-T_ENC = 23,
-ENC = 24,
-
INVALID_CMD = 10,
MOTOR_POS = 11,
@@ -174,9 +123,7 @@
NONCE_DETECT = 14,
ROTOR_ORG = 15,
-WELCOME = 20,
-
-GIVE_ENC = 100
+WELCOME = 20
};
@@ -190,40 +137,37 @@
}
void commsOutFunc() {
- int i;
-
while (true) {
osEvent evt = mail_box.get();
if (evt.status == osEventMail) {
mail_t *mail = (mail_t*)evt.value.p;
switch (mail->message) {
case KEY_DECLINED :
- pc.printf("Not a valid key!\n\r");
+ pc.printf("Not a valid key!\n\r\n\r");
break;
case ROTATION_CMD :
- pc.printf("Rotation count: %3.2f\n\r", mail->fdata);
+ pc.printf("Rotation count: %3.2f\n\r\n\r", mail->fdata);
break;
case ROTATION_FF_CMD :
- pc.printf("I will rotate forever...");
+ pc.printf("\n\rI will rotate forever...\n\r\n\r");
break;
case MAX_SPEED_CMD :
- pc.printf("Max speed: %2.1f\n\r", mail->fdata);
+ pc.printf("Max speed: %2.1f\n\r\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");
+ pc.printf("Tune command!\n\r\n\r");
break;
case INVALID_CMD :
- pc.printf("Invalid command!\r\n");
+ pc.printf("Invalid command!\r\n\n\r");
break;
case MOTOR_POS :
pc.printf("Motor position: %f\r\n", mail->fdata);
- //pc.printf("{TIMEPLOT|%.2f|Motor Position}", mail->fdata);
break;
case MOTOR_SPEED :
- pc.printf("Motor speed: %f\r\n", mail->fdata);
+ pc.printf("Motor speed: %f\r\n\n\r", mail->fdata);
break;
case NONCE_DETECT :
pc.printf("Successful nonce: %016x\n\r", mail->data);
@@ -234,32 +178,13 @@
case ROTOR_ORG :
pc.printf("Rotor origin: %x\n\r", orState);
break;
- case LEAD :
- pc.printf("Lead: %d\n\r\n\r", mail->data);
- break;
- case FINE_TUNE :
- pc.printf("Interrupts: %d\n\r\n\r", attached);
- break;
- case T_ENC :
- pc.printf("totalEncCount: %d\n\r", mail->data);
- break;
- case ENC :
- pc.printf("EncCount: %d\n\r", mail->data);
- break;
- case GIVE_ENC :
-
- for (i=0; i<encIndex; i++ ){
- pc.printf("%d\n\r", encArray[i]);
- }
- //pc.printf("%d\n\r", encIndex);
- break;
}
mail_box.free(mail);
}
}
}
-//End of block 2
+//End of that block
/***************************************************************/
/***************************************************************/
@@ -395,18 +320,13 @@
else if (command[0] == 'T') {
putMessage(TUNE_CMD);
}
- else if (command[0] == 'G') {
- putMessage(GIVE_ENC);
- }
memset(command, 0, sizeof(command));
index = 0;
- } else {
- pc.printf("Current command: %s\n\r", command); //Not sure how to go around this one cause it requires passing string
- }
+ } //else {
+ // pc.printf("Current command: %s\n\r", command); //Not sure how to go around this one cause it requires passing string
+ //}
}
}
-//End of block 3
-/***************************************************************/
void bitcoin(){
while(1) {
@@ -435,8 +355,7 @@
newKey_mutex.unlock();
sha.computeHash(hash, sequence, 64);
if (hash[0] == 0 && hash[1] == 0) {
- //putMessage(nonce);
- pc.printf("Successful nonce: %016x\n\r", *nonce);
+ putMessage(NONCE_DETECT, *nonce);
}
if ((unsigned) t.read() == currentTime) {
//pc.printf("Hash rate: %d\n\r", i - currentCount);
@@ -452,7 +371,7 @@
//Set a given drive state
-void motorOut(int8_t driveState, float pulseWidth){
+void motorOut(int8_t driveState){
//Lookup the output byte from the drive state.
int8_t driveOut = driveTable[driveState & 0x07];
@@ -485,7 +404,7 @@
int8_t motorHome() {
//Put the motor in drive state 0 and wait for it to stabilize
- motorOut(0,pulseWidth);
+ motorOut(0);
wait(2.0);
return readRotorState();
}
@@ -497,23 +416,12 @@
//orState is subtracted from future rotor state inputs to align rotor and motor states
int8_t rotorState = readRotorState();
- motorOut((rotorState-orState+lead+6)%6,pulseWidth); //+6 to make sure the remainder is positive
+ motorOut((rotorState-orState+lead+6)%6); //+6 to make sure the remainder is positive
if (rotorState - oldRotorState == 5) motorPosition --;
else if (rotorState - oldRotorState == -5) motorPosition ++;
else motorPosition += (rotorState - oldRotorState);
- //pc.printf ("motorpPosition %f\n\r", motorPosition);
+
oldRotorState = rotorState;
-
- //putMessage(ENC,encCount);
-
- if (attached == 1){
- encCount = 0;
- attached++;
- }
-
- encArray[encIndex] = encCount;
- encIndex++;
-
}
/*void push() {
intState = readRotorState();
@@ -522,34 +430,34 @@
motorOut((intState - orState + lead +6) % 6); //+6 to make sure the remainder is positive
}
}*/
+
+
void motorCtrlTick(){
motorCtrlT.signal_set(0x1);
}
+
+// Motor control
void motorCtrlFn(){
int32_t counter=0;
static int32_t oldmotorPosition;
float Ts;
+ float Tr;
float sError; //speed error
float intError = 0; //integral of velError
- float Tr;
float error;
float olderror = 0;
float Speed;
float Rev;
- float kps = 27;
- float kis = 0.4;
- float kpr = 35;
- float kdr = 14.75; //proportional constant for speed
+ float kps = 27; //proportional constant for speed
+ float kis = 0.4; //integral constant for speed
+ float kpr = 35; //proportional constant for rotation
+ float kdr = 14.75; //differential constant for speed
+
// Timer to count time passed between ticks to calculate velocity
Timer motorTime;
motorTime.start();
float motorPos;
float myTime;
-
- CHA.rise(&encISR0);
- CHB.fall(&encISR3);
- CHB.rise(&encISR1);
- CHA.fall(&encISR2);
Ticker motorCtrlTicker;
motorCtrlTicker.attach_us(&motorCtrlTick,100000);
@@ -566,40 +474,21 @@
error = Rev + motorPosition_command - motorPos;
oldmotorPosition = motorPos;
- if ((motorVelocity < 40) && (attached == 0)){
- CHA.rise(&encISR0);
- CHB.fall(&encISR3);
- CHB.rise(&encISR1);
- CHA.fall(&encISR2);
-
- attached = 1;
- }
- else if ((motorVelocity >= 60) && (attached > 0)){
- CHA.rise(NULL);
- CHB.fall(NULL);
- CHB.rise(NULL);
- CHA.fall(NULL);
-
- attached = 0;
-
- }
-
-
//equation for controls
sError = Speed -abs(motorVelocity);
- if ((motorVelocity != 0) && (abs(sError)<25)){
- intError = intError + sError;
- }
- if (abs(intError*kis)>2000) {
- intError = intError - sError;
+ if ((motorVelocity != 0) && (abs(sError)<300)){
+ intError = intError + sError;
+ if (abs(intError*kis)>2000) {
+ intError = intError - sError;
+ }
}
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)
+ if (mode == STANDARD)
{
- // Case for reverse rotation
+ // Case for forward rotation
if (error < 0){
// Case for choosing rotational control
@@ -634,7 +523,7 @@
}
}
- // Case for forward rotation
+ // Case for reverse rotation
else if (error >= 0){
// Case for choosing speed control
@@ -674,8 +563,8 @@
// ONLY speed control (aka forever forward mode)
else if (mode == FOREVER_FORWARD){
- if ( Ts >= 0) { lead = -2; }
- else if ( Ts < 0) { lead = 2; }
+ if ( Ts >= 0) { lead = 2; }
+ else if ( Ts < 0) { lead = -2; }
// Assure torque is in bounds
if (Ts > 2000){ Ts = 2000; }
@@ -693,23 +582,18 @@
//display velocity and motor position
putMessage(MOTOR_POS,0,(float)(motorPosition/6.0));
putMessage(MOTOR_SPEED,0,(float)(motorVelocity/6.0));
- //putMessage(ENC,encCount);
- //putMessage(LEAD,lead);
- putMessage(FINE_TUNE);
}
olderror=error;
}
}
int main() {
- //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);
-
- IDC = 1;
+ TestPin = 0;
// Start all threads
commsIn.start(commandProcessor);
@@ -726,5 +610,4 @@
I2.fall(&motorISR);
I3.fall(&motorISR);
-
}