Diff: main.cpp

Revision:

25:9e6e870821d8

Parent:

24:4032857546f4

Child:

26:b9f2d6d3f40e

--- a/main.cpp	Fri Mar 10 21:11:49 2017 +0000
+++ b/main.cpp	Sat Mar 11 15:03:46 2017 +0000
@@ -60,41 +60,18 @@
 const int8_t FastStateCW[7] = {0x00, 0x32, 0x2C, 0x38, 0x0B, 0x23, 0x0E};
 const int8_t FastStateACW[7] = {0x00, 0x2C, 0x0B, 0x0E, 0x32, 0x38, 0x23};
 
-//Status LED
-DigitalOut led1(LED1);
-
 //Photointerrupter inputs
 DigitalIn I1(I1pin);
-//InterruptIn I1(I1pin);
 InterruptIn I2(I2pin);
 DigitalIn I3(I3pin);
 
-InterruptIn qA(CHA);
-InterruptIn qB(CHB);
-
 //Motor Drive outputs
 DigitalOut clk(LED1);
-DigitalOut Direction(LED2);
-DigitalOut testpin(D13);
 
 //NOTE, BusOut declares things in reverse (ie, 0, 1, 2, 3) compared to binary represenation
 BusOut motor(L1Hpin, L2Lpin, L2Hpin, L3Lpin, L3Hpin);//L1Lpin,
 PwmOut singpin(L1Lpin);
 
-//BusOut digitalpins(L1Hpin, L2Lpin, L2Hpin, L3Lpin, L3Hpin);
-//PwmOut motor[6] = {L1Lpin, L1Hpin, L2Lpin, L2Hpin, L3Lpin, L3Hpin};
-//PwmOut singpin(L1Lpin);
-
-//motor[] = (singpin, digitalpins);
-
-//PwmOut L1L(L1Lpin);
-//DigitalOut L1H(L1Hpin);
-//DigitalOut L2L(L2Lpin);
-//DigitalOut L2H(L2Hpin);
-//DigitalOut L3L(L3Lpin);
-//DigitalOut L3H(L3Hpin);
-
-
 //Timeout function for rotating at set speed
 Timeout spinTimer;
 float spinWait = 10;
@@ -188,29 +165,8 @@
 
 }
 
-//void spinToPos()
-//{
-//    currRevs = 0;
-//    int remRevs = 0;
-//    revsec = 50.0;
-//    fixedSpeed();
-//    while (targetRevs - currRevs > 100) {
-//        printf("%d\r\n", currRevs);
-//    }
-//
-//    remRevs = targetRevs - currRevs;
-//    while (remRevs > 3) {
-//        revsec = remRevs/3;
-//        remRevs = targetRevs - currRevs;
-//    }
-//
-//    motorStop();
-//}
-
 void rps()
 {
-
-//    clk=!clk;
     speedTimer.stop();
     revtimer = speedTimer.read_ms();
     speedTimer.reset();
@@ -218,8 +174,6 @@
 
     measuredRevs = 1000/(revtimer);
     quadraturePosition=0;
-//    currRevs = currRevs + 1;
-
 }
 
 void VPID()
@@ -229,33 +183,11 @@
         controller.setSetPoint(revsec);
         controller.setProcessValue(measuredRevs);
         speedControl = controller.compute();
-        if(speedControl<0) speedControl = -speedControl;
-        else if (speedControl==0) speedControl = 1;
         spinWait = (1/speedControl)/6;
         Thread::wait(PIDrate);
     }
 }
 
-//void sing(float singFreq)
-//{
-//    motor = driveTable[1];
-//    wait(5.0);
-//    float singDelayus = 1000000/singFreq;
-//    for(int count=0; count<singFreq; count++) {
-//        motor = driveTable[2];
-//        wait_us(singDelayus);
-//        motor = driveTable[1];
-//    }
-//    singFreq = 15000;
-//    singDelayus = 1000000/singFreq;
-//    for(int count=0; count<singFreq; count++) {
-//        motor = driveTable[2];
-//        wait_us(singDelayus);
-//        motor = driveTable[1];
-//    }
-//}
-
-
 //Main function
 int main()
 {
@@ -273,7 +205,6 @@
     int index=0;
     int units = 0, tens = 0, decimals = 0;
     char ch;
-    testpin=0;
     int vartens = 0, varunits = 0, vardecs = 0;
     int hdrds = 0;
     float bias = 0;
@@ -285,20 +216,15 @@
 
     singpin.period_us(100);
 
-
     VPIDthread.start(VPID);
 
     while(1) {
-//        clk = I2;
-        //Toggle LED so we know something's happening
-//        clk = !clk;
 
         //If there's a character to read from the serial port
         if (pc.readable()) {
 
             //Clear index counter and control variables
             index = 0;
-//            revsec = spinWait = 0;
 
             //Read each value from the serial port until Enter key is pressed
             do {
@@ -373,78 +299,10 @@
                     break;
                     //If anything unexpected was received
 
-                case 'R':
-                    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]=='-') {
-                        spinCW = 0;
-                        //If decimal point is in the second character (eg, V-.1)
-                        if(command[2]=='.') {
-                            //Extract decimal rev/s
-                            decimals = command[3] - '0';
-
-                            //If decimal point is in the third character (eg, V-0.1)
-                        } else if(command[3]=='.') {
-                            units = command[2] - '0';
-                            decimals = command[4] - '0';
-
-                            //If decimal point is in the fourth character (eg, V-10.1)
-                        } else if(command[4]=='.') {
-                            tens = command[2] - '0';
-                            units = command[3] - '0';
-                            decimals = command[5] - '0';
-                        } else if(command[5]=='.') {
-                            hdrds = command[2] - '0';
-                            tens = command[3] - '0';
-                            units = command[4] - '0';
-                            decimals = command[6] - '0';
-                        }
-                    } else {
-                        spinCW = 1;
-                        //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';
-                        }
-                    }
-                    //Calculate the number of revolutions required
-                    targetRevs = float(hdrds)*100 + float(tens)*10 + float(units) + float(decimals)/10;
-                    singpin.period_us(float(hdrds)*100 + float(tens)*10 + float(units) + float(decimals)/10);
-                    //Print values for verification
-                    pc.printf("Target revs: %2.4f\n\r", targetRevs);
-
-                    //Run the function to start rotating at a fixed speed
-//                    spinToPos();
-                    break;
-
-
                 case 's':
-//                    pc.printf("Revs / sec: %2.2f\r", revs);
-//                    printSpeed.attach(&speedo, 1.0);
                     printf("Measured: %2.3f, revsec: %2.3f\r\n", measuredRevs, revsec);
                     printf("PID: %2.3f\r\n", speedControl);
                     break;
-                case 't':
-//                    pc.printf("%d\n\r", pos);
-                    break;
 
                 case 'K':
                     vartens = command[1] - '0';
@@ -453,7 +311,6 @@
                     Kc = float(vartens)*10 + float(varunits) + float(vardecs)/10;
                     printf("Kc: %2.1f\r\n", Kc);
                     controller.setTunings(Kc, Ti, Td);
-//                    controller.setMode(1);
                     break;
                 case 'i':
                     vartens = command[1] - '0';
@@ -462,7 +319,6 @@
                     Ti = float(vartens)*10 + float(varunits) + float(vardecs)/10;
                     printf("Ti: %2.1f\r\n", Ti);
                     controller.setTunings(Kc, Ti, Td);
-//                    controller.setMode(1);
                     break;
                 case 'd':
                     vartens = command[1] - '0';
@@ -471,7 +327,6 @@
                     Td = float(vartens)*10 + float(varunits) + float(vardecs)/10;
                     printf("Td: %2.1f\r\n", Td);
                     controller.setTunings(Kc, Ti, Td);
-//                    controller.setMode(1);
                     break;
                 case 'b':
                     if(command[1]=='.') {
@@ -493,9 +348,7 @@
                     printf("Bias: %2.1f\r\n", bias);
                     controller.setBias(bias);
                     break;
-                case 'l':
-//                    sing(261.63);
-                    break;
+
                 default:
                     //Set speed variables to zero to stop motor spinning
                     //Print error message
@@ -507,4 +360,4 @@
         }
     }
 
-}
+}
\ No newline at end of file