Ian Hua / Quadcopter-mbedRTOS

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

RTOS-Threads/src/Task4.cpp

Committer:
pHysiX
Date:
2014-05-03
Revision:
19:bd88749c8db4
Parent:
14:267368c83b6a
Child:
21:b642c18eccd1

File content as of revision 19:bd88749c8db4:

/* Update ESC (100Hz) */
/* 200Hz <= PWM frequency (400Hz) <= 400Hz */

#include "tasks.h"
#include "setup.h"

int ESCpower[4] = {0, 0, 0, 0};
int stallESC = 0;

bool armed = false;

void Task4(void const *argurment)
{
    if (armed) {
        if (counterESC) {
            /* [YAW PITCH ROLL THROTTLE AUX] */
            ESCpower[0] = (RCCommand[3]*9/10) + (pitch_adjust / 2) - (roll_adjust / 2) + (yaw_adjust / 2);
            ESCpower[1] = (RCCommand[3]*9/10) + (pitch_adjust / 2) + (roll_adjust / 2) - (yaw_adjust / 2);
            ESCpower[2] = (RCCommand[3]*9/10) - (pitch_adjust / 2) + (roll_adjust / 2) + (yaw_adjust / 2);
            ESCpower[3] = (RCCommand[3]*9/10) - (pitch_adjust / 2) - (roll_adjust / 2) - (yaw_adjust / 2);
            
            for (int i = 0; i < 4; i++)
                ESC[i] = constrainESC(ESC[i]);

            // if (ESC_check)
            //BT.printf("%4d %4d %4d %4d\n", ESCpower[0], ESCpower[1], ESCpower[2], ESCpower[3]);

            for (int i = 0; i < 4; i++)
                ESC[i].pulsewidth_us(ESCpower[i]);

            counterESC = false;
        } else {
            stallESC++;

            if (stallESC > 1) {
                imu.debugSerial.printf("ESC NOT UPDATED FAST ENOUGH!\n");
                stallESC = 0;
            }
        }
    } else {
        for (int i = 0; i < 4; i++)
            ESC[i].pulsewidth_us(ESCpower[i]);
    }

    //LED[4] = !LED[4];
}

int constrainESC(int input)
{
    if (input < 1000)
        return 1000;
    else if (input > 2000)
        return 2000;
    else
        return input;
}