Diff: RTOS-Threads/src/Task4.cpp

Revision:

55:fc7cc7a4821c

Parent:

54:a36d39a90c21

diff -r a36d39a90c21 -r fc7cc7a4821c RTOS-Threads/src/Task4.cpp
--- a/RTOS-Threads/src/Task4.cpp	Mon May 19 16:05:11 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,89 +0,0 @@
-/* File:        Task4.h
- * Author:      Trung Tin Ian HUA
- * Date:        May 2014
- * Purpose:     Thread4: ESC pulsewidth update. Note this is INDEPENDENT of the pulse frequency.
- * Settings:    400Hz
- * 200Hz <= PWM frequency <= 400Hz
- * Frequency:   400Hz
- * Refer to tasks.h to change PWM frequency
- * Timing:
- */
-#include "tasks.h"
-#include "setup.h"
-
-Semaphore sem_Task4(1);
-
-volatile int ESCpower[4] = {0, 0, 0, 0};
-int stallESC = 0;
-
-bool armed = false;
-bool ESC_check = false;
-bool calibration_mode = false;
-
-
-
-
-// ==================
-// === ESC UPDATE ===
-// ==================
-void Task4(void const *argurment)
-{
-    while (1) {
-        if (calibration_mode) {
-            if (armed)
-                for (int i = 0; i < 4; i++)
-                    ESC[i].pulsewidth_us(RCCommand[3]);
-        } else if (!armed) {
-            if (ESC_check) {
-                BT.printf("Need to ARM to check ESC output!\n");
-                ESC_check = false;
-            }
-
-            for (int i = 0; i < 4; i++) {
-                ESCpower[i] = 990;
-                ESC[i].pulsewidth_us(ESCpower[i]);
-            }
-        } else if (armed) {
-            if (RCCommand[3] < 1100) {
-                for (int i = 0; i < 4; i++) {
-                    ESCpower[i] = 1000;
-                    ESC[i].pulsewidth_us(ESCpower[i]);
-                }
-            } else {
-                //PC.printf("T4\n");
-                sem_Task4.wait();
-                //PC.printf("T4 Sem\n");
-
-                for (int i = 0; i < 3; i++)
-                    adjust[i] /= 2.0;
-
-                int throttle = RCCommand[3] * 9/10;
-
-                ESCpower[0] = constrainESC(throttle + (adjust[1]) + (adjust[2]) - adjust[0]);
-                ESCpower[1] = constrainESC(throttle + (adjust[1]) - (adjust[2]) + adjust[0]);
-                ESCpower[2] = constrainESC(throttle - (adjust[1]) - (adjust[2]) - adjust[0]);
-                ESCpower[3] = constrainESC(throttle - (adjust[1]) + (adjust[2]) + adjust[0]);
-
-                for (int i = 0; i < 4; i++)
-                    ESC[i].pulsewidth_us(ESCpower[i]);
-            } // else
-        } //else if (armed)
-        Thread::wait(TASK4_PERIOD);
-    } //while(1)
-} //Task4
-
-
-
-
-// ************************
-// *** Helper functions ***
-// ************************
-int constrainESC(float input)
-{
-    if (input < 1100.0)
-        return 1100;
-    else if (input > 2000.0)
-        return 2000;
-    else
-        return (int) input;
-}