Diff: soes.cpp

Revision:

5:6d75f432a41f

Parent:

4:bb72df6dce33

Child:

6:3ccb89a58ff8

Child:

8:09dcef3ed467

--- a/soes.cpp	Thu Dec 11 22:31:10 2014 +0000
+++ b/soes.cpp	Thu Dec 11 23:12:11 2014 +0000
@@ -44,6 +44,8 @@
 /* Private define ------------------------------------------------------------*/
 #define wd_reset 1000
 
+#define BALANCE_WIDTH 0.435
+#define BALANCE_HEIGHT 0.24
 /* Private macro -------------------------------------------------------------*/
 /* Private variables ---------------------------------------------------------*/
 _ESCvar         ESCvar;
@@ -54,17 +56,29 @@
 uint16          SM2_sml,SM3_sml;
 _Rbuffer        Rb;
 _Wbuffer        Wb;
-_Ebuffer        Eb; //EEprom
+//_Ebuffer        Eb; //EEprom
 uint8           TXPDOsize,RXPDOsize;
 uint16          wd_ok = 1, wd_cnt = wd_reset;
-volatile uint8	digoutput;
-volatile uint8	diginput;
+volatile uint8	correct_offset;
+float FrontRight_offset;
+float FrontLeft_offset;
+float BackLeft_offset;
+float BackRight_offset;
+float rawFrontLeft;
+float rawFrontRight;
+float rawBackLeft;
+float rawBackRight;
+//volatile uint8	diginput;
 
 //Serial shoe_serial(SHOE_SERIAL_TX,SHOE_SERIAL_RX);
 DigitalOut led(LED_PIN);
 DigitalOut et1100_ss(ET1100_SS);
 DigitalIn  et1100_miso(ET1100_MISO);
 SPI et1100_spi(ET1100_MOSI,ET1100_MISO,ET1100_SCK);
+AnalogIn adcFrontLeft(ADC_FL);
+AnalogIn adcFrontRight(ADC_FR);
+AnalogIn adcBackLeft(ADC_BL);
+AnalogIn adcBackRight(ADC_BR);
 
 /* Private function prototypes -----------------------------------------------*/
 /* Private functions ---------------------------------------------------------*/
@@ -78,18 +92,7 @@
 void TXPDO_update(void);
 void RXPDO_update(void);
 void DIG_process(void);
-/*
-void _delay_ms(uint16 number_of_ms)
-{
-    volatile uint16_t cpu_counter = 0;
-    uint16_t ms_counter;
-    for(ms_counter = 0 ; ms_counter < number_of_ms ; ms_counter++)
-    {
-        cpu_counter = 48000; //48MHz processor
-        while(cpu_counter >0)
-            cpu_counter--;
-    }
-}*/
+void sample(void);
 
 void ESC_objecthandler(uint16 index, uint8 subindex)
 {
@@ -101,10 +104,10 @@
         switch (subindex)
         {
         case 0x01:
-            dummy8  = Eb.setting8;//Write value to EEPROM; eeprom_write_byte(&eedat.setting8, Wb.setting8);
+            dummy8  = 0;//Eb.setting8;//Write value to EEPROM; eeprom_write_byte(&eedat.setting8, Wb.setting8);
             break;
         case 0x02:
-            dummy16 = Eb.setting16;//Write value to EEPROM; eeprom_write_word(&eedat.setting16, Wb.setting16);
+            dummy16 = 0;//Eb.setting16;//Write value to EEPROM; eeprom_write_word(&eedat.setting16, Wb.setting16);
             break;
         }
         break;
@@ -137,16 +140,18 @@
           ESCvar.ALevent &= ~ESCREG_ALEVENT_SM2;
             RXPDO_update();
             // dummy output point
-            digoutput = Wb.digoutput;
-            if(digoutput & 0x01)
+            correct_offset = Wb.correct_offset;
+            if(correct_offset & 0x01) {
                 led.write(1);
-            else
-                led.write(0);
+                FrontLeft_offset = adcFrontLeft;
+                FrontRight_offset = adcFrontRight;
+                BackLeft_offset = adcBackLeft;
+                BackRight_offset = adcBackRight;
+            }
             wd_cnt = wd_reset;
         }
 
-      if (!wd_cnt)
-        {
+        if (!wd_cnt) {
             ESC_stopoutput();
             // watchdog, invalid outputs
             ESC_ALerror(ALERR_WATCHDOG);
@@ -160,25 +165,39 @@
         wd_cnt = wd_reset;
     }
   if (APPstate) //input or output enabled
-    {
+    {	
+    	float fl,fr,br,bl,copx,copy;
         Rb.timestamp = ESCvar.Time;
         //just some dummy data to test
         //Rb.counter++;
         //Rb.diginput = diginput;
         //Rb.analog[0] = 1;
         //Rb.analog[1] = 2;
-        Rb.CoPx = 3;
-        Rb.CoPy = 4;
-        Rb.FrontRight = 5;
-        Rb.FrontLeft = 6;
-        Rb.BackRight = 7;
-        Rb.BackLeft = 8;
+        fr = rawFrontRight - FrontRight_offset;
+        fl = rawFrontLeft - FrontLeft_offset;
+        br = rawBackRight - BackRight_offset;
+        bl = rawBackLeft - BackLeft_offset;
+        copx = ((fl+bl)-(fr+br))*BALANCE_WIDTH;
+        copy = ((fr+fl)-(br+bl))*BALANCE_HEIGHT;
+        Rb.CoPx = copx;
+        Rb.CoPy = copy;
+        Rb.FrontRight = rawFrontRight;
+        Rb.FrontLeft = rawFrontLeft;
+        Rb.BackRight = rawBackRight;
+        Rb.BackLeft = rawBackLeft;
 
         TXPDO_update();
-    }
+    } 
 }
 
-
+void sample(void)
+{
+	rawFrontLeft = adcFrontLeft;
+	rawFrontRight = adcFrontRight;
+	rawBackLeft = adcBackLeft;
+	rawBackRight = adcBackRight;
+	
+}
 int main(void)
 {
     /*!< At this stage the microcontroller clock setting is already configured,
@@ -187,13 +206,15 @@
          To reconfigure the default setting of SystemInit() function, refer to
          system_stm32f0xx.c file
        */
+    Ticker adc_sampler;
     cpuinit();
+    adc_sampler.attach(sample,0.001);
 	TXPDOsize = sizeTXPDO();
 	RXPDOsize = sizeRXPDO();
     wait_ms(200);
     /*initialize configuration*/
-    Eb.setting16 = 0xABCD;
-    Eb.setting8  = 111;
+    //Eb.setting16 = 0xABCD;
+    //Eb.setting8  = 111;
     // wait until ESC is started up
     while ((ESCvar.DLstatus & 0x0001) == 0)
         ESC_read(ESCREG_DLSTATUS, &ESCvar.DLstatus, sizeof(ESCvar.DLstatus), &ESCvar.ALevent);