Diff: main.cpp

Revision:

27:206f781756f9

Parent:

26:b9f2d6d3f40e

--- a/main.cpp	Sat Mar 11 15:59:37 2017 +0000
+++ b/main.cpp	Tue Mar 14 18:29:47 2017 +0000
@@ -4,10 +4,10 @@
 #include "PID.h"
 
 //PID controller configuration
-float PIDrate = 0.8;
+float PIDrate = 0.2;
 float Kc = 10.0;
 float Ti = 3.0;
-float Td = 2.5;
+float Td = 0;
 float speedControl = 0;
 PID controller(Kc, Ti, Td, PIDrate);
 Thread VPIDthread;
@@ -62,8 +62,8 @@
 
 //Photointerrupter inputs
 DigitalIn I1(I1pin);
-InterruptIn I2(I2pin);
-DigitalIn I3(I3pin);
+DigitalIn I2(I2pin);
+InterruptIn I3(I3pin);
 
 //Motor Drive outputs
 DigitalOut clk(LED1);
@@ -100,10 +100,11 @@
 void motorOut(int8_t driveState)
 {
 
-    motor = 0x2A;
+    motor = 0x2A>>1;
+    singpin = 0;
 
     if(revtimer<33) {
-        clk=1;
+//        clk=1;
         if(!spinCW) {
             motor = (FastStateACW[driveState]>>1);
             singpin = float(FastStateACW[driveState]&&0x01)/2;
@@ -112,7 +113,7 @@
             singpin = float(FastStateCW[driveState]&&0x01)/2;
         }
     } else {
-        clk=0;
+//        clk=0;
         if(!spinCW) {
             motor = (AcwState[driveState]>>1);
             singpin = float(AcwState[driveState]&&0x01)/2;
@@ -158,12 +159,15 @@
     //If spinning is required, attach the necessary wait to the
     //timeout interrupt to call this function again and
     //keep the motor spinning at the right speed
-    if(revsec) spinTimer.attach(&fixedSpeed, spinWait);
+
 
     intState = readRotorState();
     //Increment state machine to next state
     motorOut(intState);
 
+//    motorOut(I1 + I2<<1 + I3<<2);
+    if(revsec) spinTimer.attach(&fixedSpeed, spinWait);
+
 }
 
 void rps()
@@ -179,13 +183,13 @@
 
 void VPID()
 {
-    controller.setMode(1);
     while(1) {
-        controller.setSetPoint(revsec);
         controller.setProcessValue(measuredRevs);
         speedControl = controller.compute();
-        spinWait = (1/speedControl)/6;
-        Thread::wait(PIDrate);
+//        if(speedControl<measuredRevs) speedControl = measuredRevs/2;
+        spinWait = (1/(speedControl*6));
+//        Thread::wait(PIDrate);
+        wait(PIDrate);
     }
 }
 
@@ -212,12 +216,17 @@
 
     speedTimer.reset();
     speedTimer.start();
-    I2.mode(PullNone);
-    I2.fall(&rps);
+    I3.mode(PullNone);
+    I3.rise(&rps);
 
     singpin.period_us(100);
 
     VPIDthread.start(VPID);
+    pc.printf("%d", VPIDthread.get_priority());
+    VPIDthread.set_priority(osPriorityAboveNormal);
+
+    controller.setInterval(PIDrate);
+    controller.setMode(0);
 
     while(1) {
 
@@ -246,7 +255,7 @@
             switch (command[0]) {
                     //If a V was typed...
                 case 'V':
-                    units = 0, tens = 0, decimals = 0;
+                    hdrds = 0, units = 0, tens = 0, decimals = 0;
                     //For each character received, subtract ASCII 0 from ASCII
                     //representation to obtain the integer value of the number
                     if(command[1]=='-') {
@@ -266,6 +275,11 @@
                             tens = command[2] - '0';
                             units = command[3] - '0';
                             decimals = command[5] - '0';
+                        } else if(command[4]=='.') {
+                            hdrds = command[1] - '0';
+                            tens = command[2] - '0';
+                            units = command[3] - '0';
+                            decimals = command[5] - '0';
                         }
                     } else {
                         spinCW = 1;
@@ -284,14 +298,20 @@
                             tens = command[1] - '0';
                             units = command[2] - '0';
                             decimals = command[4] - '0';
+                        } else if(command[4]=='.') {
+                            hdrds = command[1] - '0';
+                            tens = command[2] - '0';
+                            units = command[3] - '0';
+                            decimals = command[5] - '0';
                         }
                     }
 
                     //Calculate the number of revolutions per second required
-                    revsec = float(tens)*10 + float(units) + float(decimals)/10;
+                    revsec = float(hdrds)*100 + float(tens)*10 + float(units) + float(decimals)/10;
                     //Calculate the required wait period
 
                     spinWait = (1/revsec)/6;
+                    controller.setSetPoint(revsec);
                     //Print values for verification
                     pc.printf("Rev/S: %2.4f\n\r", revsec);
 
@@ -334,6 +354,8 @@
                     varunits = command[2] - '0';
                     vardecs = command[4] - '0';
                     PIDrate = float(vartens)*10 + float(varunits) + float(vardecs)/10;
+                    controller.setInterval(PIDrate);
+                    controller.setMode(1);
                     printf("Rate: %2.1f\r\n", PIDrate);
                 case 'b':
                     if(command[1]=='.') {
@@ -356,6 +378,33 @@
                     controller.setBias(bias);
                     break;
 
+                case 'T':
+                    hdrds = 0, units = 0, tens = 0, decimals = 0;
+
+                    //If decimal point is in the second character (eg, V.1)
+                    if(command[1]=='.') {
+                        //Extract decimal rev/s
+                        decimals = command[2] - '0';
+
+                        //If decimal point is in the third character (eg, V0.1)
+                    } else if(command[2]=='.') {
+                        units = command[1] - '0';
+                        decimals = command[3] - '0';
+
+                        //If decimal point is in the fourth character (eg, V10.1)
+                    } else if(command[3]=='.') {
+                        tens = command[1] - '0';
+                        units = command[2] - '0';
+                        decimals = command[4] - '0';
+                    } else if(command[4]=='.') {
+                        hdrds = command[1] - '0';
+                        tens = command[2] - '0';
+                        units = command[3] - '0';
+                        decimals = command[5] - '0';
+                    }
+                    singpin.period_us(float(hdrds)*100 + float(tens)*10 + float(units) + float(decimals)/10);
+                    break;
+
                 default:
                     //Set speed variables to zero to stop motor spinning
                     //Print error message
@@ -365,6 +414,7 @@
                     break;
             }
         }
+        wait(0.01);
     }
 
 }
\ No newline at end of file