Diff: main.cpp

Revision:

18:5e9b288793bb

Parent:

17:eb74805e8f9b

Child:

19:a71ef54d9e94

--- a/main.cpp	Tue Aug 17 11:27:34 2021 +0000
+++ b/main.cpp	Thu Nov 04 09:17:40 2021 +0000
@@ -30,7 +30,8 @@
 
 double SPIN_T[6]; //times for spin cycle
 
-QEI encoder(PA_14, PA_13, PA_15, 256, QEI::X4_ENCODING);
+//QEI encoder(PA_14, PA_13, PA_15, 256, QEI::X4_ENCODING);
+QEI encoder(PA_15, PB_7, PC_13, 256, QEI::X4_ENCODING);
 
 //LIS3DH accel = LIS3DH(SPI_MOSI, SPI_MISO, SPI_SCLK, SPI_CS,LIS3DH_DR_NR_LP_400HZ, LIS3DH_FS_2G);
 
@@ -53,13 +54,13 @@
 
 DigitalOut pinSt(A3);
 
-//Serial pc(USBTX,USBRX);
+Serial pc(USBTX,USBRX);
 
 DigitalOut      pinStatusLED    (D13);
 
-DigitalIn       pinButton       (PC_8);
-DigitalOut      pinLedWhite     (PC_6);
-DigitalOut      pinLedRed       (PC_5);
+DigitalIn       pinButton       (PC_11);//(PC_8);
+DigitalOut      pinLedWhite     (PC_2);//(PC_6);
+DigitalOut      pinLedRed       (PC_3);//(PC_5);
 
 /* Still to allocate
 
@@ -233,82 +234,86 @@
             }
         }
             
-            if(state == STATE_ERROR){
-                demandSpeed_RPM -=66.7*SAMPLE_TIME_US/1000000;
-            }
+        if(state == STATE_ERROR){
+            demandSpeed_RPM -=66.7*SAMPLE_TIME_US/1000000;
+        }
+    
+        
+        demandSpeed_RPM = LimitDouble(demandSpeed_RPM,0.0,MAX_SPEED_RPM);//limit demand
+        currentPulses = encoder.getPulses();//calculate current speed
+        double deltaPulses = currentPulses - lastPulses;
+        double speed_RPM = deltaPulses / SAMPLE_TIME_US * 1000000;//get in pulses per second
+        speed_RPM *= 60/4/PULSES_PER_REV;//convert to RPM
+        
+        currentSpeedRPM = 0.1*speed_RPM + 0.9*lastSpeedRPM;//filter speed
         
-            
-            demandSpeed_RPM = LimitDouble(demandSpeed_RPM,0.0,MAX_SPEED_RPM);//limit demand
-            currentPulses = encoder.getPulses();//calculate current speed
-            double deltaPulses = currentPulses - lastPulses;
-            double speed_RPM = deltaPulses / SAMPLE_TIME_US * 1000000;//get in pulses per second
-            speed_RPM *= 60/4/PULSES_PER_REV;//convert to RPM
-            
-            currentSpeedRPM = 0.1*speed_RPM + 0.9*lastSpeedRPM;//filter speed
-            
-            double error = demandSpeed_RPM - currentSpeedRPM;//calculate error
-            deltaError = error - lastError;
-            double errorDot = deltaError/SAMPLE_TIME_US*1000000.0;
-            errorDot = 0.1*errorDot + 0.9*lastErrorDot;
-            integralTerm += error*Ki*SAMPLE_TIME_US/1000000.0;
-            integralTerm = LimitDouble(integralTerm,-0.8, 0.8);
-            
-            output = Kp * error; //calculate output
-            output += integralTerm;
-            output += Kd*errorDot;
-            output += Ko*demandSpeed_RPM;
-            output = LimitDouble(output,-1.0,1.0);
-            //printf("wd:%f\t w:%f\t e:%f\t o:%f \r\n",demandSpeed_RPM, currentSpeedRPM, error, output);
+        double error = demandSpeed_RPM - currentSpeedRPM;//calculate error
+        deltaError = error - lastError;
+        double errorDot = deltaError/SAMPLE_TIME_US*1000000.0;
+        errorDot = 0.1*errorDot + 0.9*lastErrorDot;
+        integralTerm += error*Ki*SAMPLE_TIME_US/1000000.0;
+        integralTerm = LimitDouble(integralTerm,-0.8, 0.8);
         
-            if(output >=0){//Set direction
-                PHA.write(0);
-            }else{
-                PHA.write(1);
-                output = -1*output;
-            }
-            
+        output = Kp * error; //calculate output
+        output += integralTerm;
+        output += Kd*errorDot;
+        output += Ko*demandSpeed_RPM;
+        output = LimitDouble(output,-1.0,1.0);
+        //printf("wd:%f\t w:%f\t e:%f\t o:%f \r\n",demandSpeed_RPM, currentSpeedRPM, error, output);
+    
+        if(output >=0){//Set direction
+            PHA.write(0);
+        }else{
+            PHA.write(1);
+            output = -1*output;
+        }
+        
+//        if(demandSpeed_RPM = 0.0){
+//            output = 0.0;
+//        }
+        
 //            if(state == STATE_ERROR){
 //                output = 0;
 //            }
-                
-            PWMA.write(output);//write to motor
-            lastPulses = currentPulses;//update 
-            lastError = error;
-            lastSpeedRPM = currentSpeedRPM;
-            lastErrorDot = errorDot;
+            
+        PWMA.write(output);//write to motor
+        lastPulses = currentPulses;//update 
+        lastError = error;
+        lastSpeedRPM = currentSpeedRPM;
+        lastErrorDot = errorDot;
+        
+        //exit when test has completed
+        if (Tnow >= SPIN_T[5]){
+            //printf("Terminating Test %f \t %f\r\n", Tnow, SPIN_T[5]);
+            state = STATE_READY;//change state
+            testTimer.stop(); //stop and reset timers
+            testTimer.reset();
+            Tnow = testTimer.read();
+            encoderTimer.stop();
+            encoderTimer.reset();
+            PWMA.write(0.0);
             
-            //exit when test has completed
-            if (Tnow >= SPIN_T[5]){
-                //printf("Terminating Test %f \t %f\r\n", Tnow, SPIN_T[5]);
-                state = STATE_READY;//change state
-                testTimer.stop(); //stop and reset timers
-                testTimer.reset();
-                Tnow = testTimer.read();
-                encoderTimer.stop();
-                encoderTimer.reset();
-                PWMA.write(0.0);
-                
-                //add check if motor has stopped
-                //printf("Wating for motor to stop.\r\n");
-                while(isSpinning()){
-                    ThisThread::sleep_for(200);
-                }
-                //EN_FAULTA.write(0);//disable motor
-                //EN_FAULTB.write(0);
-                
-                tickerMotorControl.detach(); //detach the semaphore release for motor control
-                tickerPrint.detach();
-                
-                //printf("state = %d\r\n",state);
-                pinLedRed = 0;
-                TestCompleteNotification();//send notification
-                lastErrorDot = 0.0;
-                lastError = 0.0;
-                integralTerm = 0.0;
-                //deactivate motor
-                //PrintThread.terminate();//terminate print thread
-                //CentrifugeTestThread.terminate();//terminate threads
+            //add check if motor has stopped
+            //printf("Wating for motor to stop.\r\n");
+            while(isSpinning()){
+                ThisThread::sleep_for(200);
             }
+            //EN_FAULTA.write(0);//disable motor
+            //EN_FAULTB.write(0);
+            
+            tickerMotorControl.detach(); //detach the semaphore release for motor control
+            tickerPrint.detach();
+            
+            //printf("state = %d\r\n",state);
+            pinLedRed = 0;
+            TestCompleteNotification();//send notification
+            lastErrorDot = 0.0;
+            lastError = 0.0;
+            integralTerm = 0.0;
+            //deactivate motor
+            //PrintThread.terminate();//terminate print thread
+            //CentrifugeTestThread.terminate();//terminate threads
+        }
             
         //} //end running conditional
  
@@ -332,7 +337,7 @@
         encoderTimer.start();
         PrintThread.start(PrintTimeRemaining);
         //printf("\r\n Test setup complete, State:%d \r\n", state);
-        EN_FAULTA.write(1);
+        EN_FAULTA.write(1);//enable motor
 
         //EN_FAULTB.write(1);
         if(state == STATE_RUNNING){
@@ -441,7 +446,7 @@
 }
 
 int main(){
-    //printf("\r\nSystem Online\r\n");
+    printf("\r\nSystem Online\r\n");
     //set up system
     
     //set up motor