Rc controll is working

adamkaliszan · Jun 14, 2020 · 6ce379e · 6ce379e
1 parent 2a0a384
commit 6ce379e
Show file tree

Hide file tree

Showing 7 changed files with 62 additions and 34 deletions.
diff --git a/Lib/include/rc_ibus.h b/Lib/include/rc_ibus.h
@@ -3,6 +3,8 @@
 
 #include <stdint.h>
 
+extern int8_t rcChannels[8];
+
 uint8_t ibus_processDta(uint8_t dta);
 void ibus_processMessage(void);
 

diff --git a/Lib/rc_ibus.c b/Lib/rc_ibus.c
@@ -61,9 +61,17 @@ void ibus_newFrame()
     _state = RC_ST_SIZE;
 }
 
+int8_t rcChannels[8];
+
+
 void ibus_processMessage(void)
 {
-
+    uint8_t i;
+    if (message.type == 0x40)
+    {
+        for (i=0; i<8; i++)
+            rcChannels[i] = (1500 - message.channels[i]) / 4;
+    }
 }
 
 uint8_t ibus_processDta(uint8_t dta)

diff --git a/Projects/AtXmega/RoverController.X/hardware.c b/Projects/AtXmega/RoverController.X/hardware.c
@@ -81,7 +81,7 @@ void hardwareInit(void)
 
 
     //Timer 0 H bridge PWM
-    TCC0.PER   = 100;   //Zakres
+    TCC0.PER   = 127;   //Zakres
 
     ///LoadConfig overrides this register
     TCC0.CTRLA = 0x07;      //Preskaler 1024 PWM @ 0.32 KHz Za niska częstotliwość
@@ -135,36 +135,36 @@ void forwardA(uint8_t left, uint8_t right)
 }
 
 
-void backwordA(uint8_t left, uint8_t right)
-{
-    PORTR.OUTSET = 0x02;
-    PORTA.OUTCLR = 0x90;
-    PORTA.OUTSET = 0x60;
+//void backwordA(uint8_t left, uint8_t right)
+//{
+//    PORTR.OUTSET = 0x02;
+//    PORTA.OUTCLR = 0x90;
+//    PORTA.OUTSET = 0x60;
 
-    TCC0.CCA = left;
-    TCC0.CCB = right;
-}
+//    TCC0.CCA = left;
+//    TCC0.CCB = right;
+//}
 
-void rotateLeftA(uint8_t left, uint8_t right)
-{
-    PORTR.OUTSET = 0x02;
-    PORTA.OUTCLR = 0xA0;
-    PORTA.OUTSET = 0x50;
+//void rotateLeftA(uint8_t left, uint8_t right)
+//{
+//    PORTR.OUTSET = 0x02;
+//    PORTA.OUTCLR = 0xA0;
+//    PORTA.OUTSET = 0x50;
 
-    TCC0.CCA = left;
-    TCC0.CCB = right;
-}
+//    TCC0.CCA = left;
+//    TCC0.CCB = right;
+//}
 
-void rotateRightA(uint8_t left, uint8_t right)
-{
-    PORTR.OUTSET = 0x02;
+//void rotateRightA(uint8_t left, uint8_t right)
+//{
+//    PORTR.OUTSET = 0x02;
 
-    PORTA.OUTCLR = 0x50;
-    PORTA.OUTSET = 0xA0;
+//    PORTA.OUTCLR = 0x50;
+//    PORTA.OUTSET = 0xA0;
 
-    TCC0.CCA = left;
-    TCC0.CCB = right;
-}
+//    TCC0.CCA = left;
+//    TCC0.CCB = right;
+//}
 
 //0 - (18)        4 - B1 in
 //1 - (17)        5 - A1 in

diff --git a/Projects/AtXmega/RoverController.X/hardware.h b/Projects/AtXmega/RoverController.X/hardware.h
@@ -40,10 +40,10 @@ void hardwareInit(void);
 
 void offHbridge(void);
 
-void forwardA(uint8_t left, uint8_t right);
-void backwordA(uint8_t left, uint8_t right);
-void rotateLeftA(uint8_t left, uint8_t right);
-void rotateRightA(uint8_t left, uint8_t right);
+//void forwardA(uint8_t left, uint8_t right);
+//void backwordA(uint8_t left, uint8_t right);
+//void rotateLeftA(uint8_t left, uint8_t right);
+//void rotateRightA(uint8_t left, uint8_t right);
 
 void forwardB(uint8_t left, uint8_t right);
 void backwordB(uint8_t left, uint8_t right);

diff --git a/Projects/AtXmega/RoverController.X/main.c b/Projects/AtXmega/RoverController.X/main.c
@@ -143,7 +143,7 @@ portSHORT main( void )
 
   xTaskCreate(vTaskRc,        "RcRec",         2000,          NULL,                                   2, NULL);
   xTaskCreate(vTaskVTYusb,    "VTY" ,         1000+STACK_SIZE_VTY,         (void *)(CLIStateSerialUsb),            1, &xHandleVTY_USB);
-//  xTaskCreate(vTaskTLV,       NULL /*"TLV"            */, STACK_SIZE_VTY,         (void *)(TLVstate),                     1, &xHandleTLV);
+  xTaskCreate(vTaskTLV,       NULL /*"TLV"            */, STACK_SIZE_VTY,         (void *)(TLVstate),                     1, &xHandleTLV);
 //  xTaskCreate(vTaskMain,      NULL /*"TLV"            */, STACK_SIZE_VTY,         NULL,                                   1, &xHandleMain);
 
   vTaskStartScheduler();

diff --git a/Projects/AtXmega/RoverController.X/rc_task.c b/Projects/AtXmega/RoverController.X/rc_task.c
@@ -10,6 +10,7 @@
 #include "../../../Lib/include/rc_ibus.h"
 
 #include "hardwareConfig.h"
+#include "hardware.h"
 
 
 extern xQueueHandle xSIM900Rec;
@@ -23,14 +24,32 @@ void vTaskRc(void *ptr)
     uint8_t tmp;
     for (;;)
     {
-        if (xQueueReceive(xSIM900Rec, &tmp, 1) == pdFALSE)
+        if (xQueueReceive(xSIM900Rec, &tmp, 50) == pdFALSE)
         {
-            ibus_newFrame();
+            pwrOff4v3();
+            vTaskDelay(500);
+            pwrOn4v3rpi();
             continue;
         }
         if (!ibus_processDta(tmp))
             continue;
 
         ibus_processMessage();
+
+        int16_t fwL = rcChannels[2] + rcChannels[3];
+        int16_t fwR = rcChannels[2] - rcChannels[3];
+
+        if (fwL > 127) fwL = 127;
+        if (fwL < -127) fwL = -127;
+
+        if (fwR > 127) fwR = 127;
+        if (fwR < -127) fwR = -127;
+
+        if ((fwL >= 0) && (fwR >= 0)) forwardB(fwL, fwR);
+        if ((fwL <  0) && (fwR <  0)) backwordB(-fwL, -fwR);
+
+        if ((fwL >= 0) && (fwR <  0)) rotateRightB(fwL, -fwR);
+        if ((fwL <  0) && (fwR >= 0)) rotateLeftB(-fwL, fwR);
+
     }
 }
diff --git a/Projects/AtXmega/RoverController.X/tlv_task.c b/Projects/AtXmega/RoverController.X/tlv_task.c
@@ -72,4 +72,3 @@ void vTaskTLV(void *tlvIntPtr)
     xSemaphoreGive(Hc12semaphore);
   }
 }
-
Original file line number	Diff line number	Diff line change
Expand Up		@@ -72,4 +72,3 @@ void vTaskTLV(void *tlvIntPtr)
		xSemaphoreGive(Hc12semaphore);
		}
		}