Diff: main.cpp

Revision:

2:5eacc17e53eb

Parent:

1:57efa2224451

--- a/main.cpp	Fri May 24 08:39:03 2013 +0000
+++ b/main.cpp	Wed May 29 07:36:36 2013 +0000
@@ -10,7 +10,7 @@
 Serial pc(USBTX, USBRX); // tx, rx
 DigitalOut led1(LED1);  // main led
 DigitalOut led2(LED2);  // Interrupt led
-PwmOut led3(LED3), aPwm0(p23);
+PwmOut led3(LED3), aPwm0(p21), aPwm1(p22), aPwm2(p23);
 
 Ticker t;
 
@@ -19,15 +19,25 @@
 Timer tmr1;
 AnalogIn AD1(p20), AD2(p19);
 
-unsigned int ServoCnt, BackgrCnt, ServoTime; 
+unsigned int ServoCnt =0, BackgrCnt =0, ServoTime =0; 
 float Potm1, Potm2;
+float Potm1Offset = 0.5;                // Center position is speed zero
 
-int TableIndex;
+int TableIndex0 =0, TableIndex1 =0, TableIndex2 =0;                         // Index in sine table 
+float fTableIndex0 =0, fTableIndex1 =0,  fTableIndex2 =0, SpeedIndex =0;           // Speedindex is calculated dependinding on cycle speed. fTableIndex0 is the floating point index progressing 
+
 int SineOut0, SineOut1, SineOut2;       // The Sine output values
 float mSineOut0, mSineOut1, mSineOut2;  // The multiplied Sine output values
-
+float MotorSpeed, CycleSpeed;           // Motor speed in cycles per second derived from potm1
+float MaxMotorSpeed = 10;
+float MaxCycleSpeed = 10;
 
-int PwmTime_us = 10000;
+int Rti_us = 100000;                     // Real time interrupt time in us
+float Rti_sec = 0.33;                    // The cycle time
+
+// Change PWM freq HERE
+int PwmTime_us = 50, PwmPulsWidth0 = 0, PwmPulsWidth1 = 0, PwmPulsWidth2 = 0;  // Reference time Pwm and actual puls time
+//int PwmTime_us = 10000, PwmPulsWidth0 = 0, PwmPulsWidth1 = 0, PwmPulsWidth2 = 0;  // Reference time Pwm and actual puls time
 int Sin1Offset = 120, Sin2Offset = 240;  // Offsets for two other phase outputs
 
 int SineTable[360] = {0, 17, 35, 52, 70, 87, 105, 122, 139, 156 
@@ -79,17 +89,54 @@
     {int starttime, endtime;
     
         starttime = tmr1.read_us();         // Measure time 1
-        _pin = !_pin;                       // toggle led 2
         
         Potm1 = AD1.read(); Potm2 = AD2.read();  // Get potmeters
-        led3.pulsewidth_us(Potm1 * PwmTime_us);     // Set duty cycle for LED
-        aPwm0.pulsewidth_us(Potm1 * PwmTime_us);    // Set DC amp out 0
+   //     led3.pulsewidth_us(Potm1 * PwmTime_us);     // Set duty cycle for LED
+        
+        CycleSpeed = (Potm1 - Potm1Offset) * 30 * MaxCycleSpeed;  // Potmeter -0.5 to +0.5
+        SpeedIndex = CycleSpeed;
+        fTableIndex0 = fTableIndex0 + SpeedIndex;
+        fTableIndex1 = fTableIndex0+120;    // calculate offset angles other two phases
+        fTableIndex2 = fTableIndex1+120;
+        
+        while (fTableIndex0 < 0) { fTableIndex0 += 360; };           // Caclulate modulo 360 index Ph0
+        while (fTableIndex0 > 359) { fTableIndex0 -= 360; };
+     //   fTableIndex1 = fTableIndex1 % 359;
+        while (fTableIndex1 < 0) { fTableIndex1 += 360; };           // Caclulate modulo 360 index ph1
+        while (fTableIndex1 > 359) { fTableIndex1 -= 360; };
+        while (fTableIndex2 < 0) { fTableIndex2 += 359; };           // Caclulate modulo 360 index ph2
+        while (fTableIndex2 > 359) { fTableIndex2 -= 360; };
         
-        TableIndex = ceil(Potm1 * 359);
-        SineOut0 = SineTable[TableIndex];   // Get sine value from table
+        TableIndex0 = std::floor(fTableIndex0);
+        TableIndex1 = std::floor(fTableIndex1); 
+        TableIndex2 = std::floor(fTableIndex2);
+
+        SineOut0 = SineTable[TableIndex0];   // Get sine value from table  ph0
+        mSineOut0 = SineOut0 * Potm2;        // Multiply sine with potmeter
+        SineOut1 = SineTable[TableIndex1];   // Get sine value from table  ph1
+        mSineOut1 = SineOut1 * Potm2;        // Multiply sine with potmeter
+        SineOut2 = SineTable[TableIndex2];   // Get sine value from table  ph2
+        mSineOut2 = SineOut2 * Potm2;        // Multiply sine with potmeter
+        
+//        PwmPulsWidth = PwmTime_us * TableIndex0/360;
+        PwmPulsWidth0 = PwmTime_us * (mSineOut0 + 1000) /2000;   // output based on sine table via degree index multiplied by potm2 and offset 50% 
+        PwmPulsWidth1 = PwmTime_us * (mSineOut1 + 1000) /2000;   // output based on sine table via degree index multiplied by potm2 and offset 50%
+        PwmPulsWidth2 = PwmTime_us * (mSineOut2 + 1000) /2000;   // output based on sine table via degree index multiplied by potm2 and offset 50%
+        aPwm0.pulsewidth_us(PwmPulsWidth0);    // Set DC amp out 0
+        aPwm1.pulsewidth_us(PwmPulsWidth1);    // Set DC amp out 0
+        aPwm2.pulsewidth_us(PwmPulsWidth2);    // Set DC amp out 0
+        //aPwm0.pulsewidth_us(Potm1 * PwmTime_us);    // Set DC amp out 0
+     //   led3.pulsewidth_us(0.4 * PwmTime_us);     // Set duty cycle for LED
+        led3.pulsewidth_us(PwmPulsWidth0);     // Set duty cycle for LED
+    //    led3.pulsewidth_us(Potm1 * PwmTime_us);     // Set duty cycle for LED
+        
+     //   TableIndex0 = ceil(Potm1 * 359);
+        SineOut0 = SineTable[TableIndex0];   // Get sine value from table
         mSineOut0 = SineOut0 * Potm2;        // Multiply sine with potmeter
         
         ServoCnt++;                         // Servo counter
+        if ((ServoCnt % 10) == 0) {_pin = !_pin;};     // toggle led 2
+
         endtime = tmr1.read_us();           // Measure time 2
         ServoTime = endtime - starttime;
     }
@@ -103,15 +150,18 @@
 void DoInit(void) {
     pc.baud(115200);
     tmr1.start();
-
-    t.attach_us(&f, &Flipper::flip, 100000); // the address of the object, member function, and interval
+  //  Rti_sec = Rti_us / 1000000;
+    
+    t.attach_us(&f, &Flipper::flip, Rti_us); // the address of the object, member function, and interval
 //    t.attach_us(&f, &Flipper::flip, 25); // the address of the object, member function, and interval
                                             // 25 us = 40kHz
 //    led3 = 1;
     led3.period_us(PwmTime_us);            // Set the period to 10mS = 100Hz
-//    aPwm0.period_ms(PwmTime_us);    // Set the period for the pwm output to amplifier
+//    aPwm0.period_us(PwmTime_us);    // Set the period for the pwm output to amplifier
+//    aPwm1.period_us(PwmTime_us);    // Set the period for the pwm output to amplifier
+//    aPwm2.period_us(PwmTime_us);    // Set the period for the pwm output to amplifier
 
-    pc.printf("The Init time was %f seconds\n\r", tmr1.read());
+    pc.printf("The Init time was %f seconds, Cycletime: %f\n\r", tmr1.read(), Rti_sec);
   //  Return 0;
 }
 
@@ -125,8 +175,14 @@
         wait(0.2);
         led1 = 0;
         wait(0.2);
-        pc.printf("Servotm: %i, ServoCnt: %i , BackgrCnt: %i , Potm1: %.2f, Potm2: %i, TableIndex :%i, SineOut0: %i , mSineOut0: %.2f\r",
-         ServoTime, ServoCnt, BackgrCnt, Potm1, Potm2, TableIndex, SineOut0, mSineOut0 );
+        pc.printf("Servotm: %i, ServoCnt: %i , BackgrCnt: %i , Potm1: %.2f, Potm2: %.2f \r",
+         ServoTime, ServoCnt, BackgrCnt, Potm1, Potm2);
+        pc.printf("TableIndex0 :%i, TableIndex0 :%i, TableIndex0 :%i, SineOut0: %i , mSineOut0: %.2f, mSineOut1: %.2f, mSineOut2: %.2f \r",
+         ServoTime, TableIndex0, TableIndex1, TableIndex2, SineOut0, mSineOut0, mSineOut1 , mSineOut2  );
+        pc.printf("CycleSpeed: %.2f, fTableIndex0: %.2f, fTableIndex1: %.2f, fTableIndex2: %.2f  \r",
+         CycleSpeed, fTableIndex0,  fTableIndex1, fTableIndex2);
+        pc.printf("PwmPulsWidth0: %i, PwmPulsWidth1: %i ,, PwmPulsWidth2: %i \r\r",
+         PwmPulsWidth0, PwmPulsWidth1, PwmPulsWidth2 );
     }
 }