File content as of revision 4:81cb68f1bcbd:

#include "mbed.h"
#include "pins.h"
#include "motor.h"
#include "USBSerial.h"
#include "RFManager.h"

#define BUF_SIZE 32

USBSerial serial;

Serial pc(USBTX, USBRX);

DigitalOut led1(LED_1);
DigitalOut led2(LED_2);
DigitalOut led3(LED_3);

static const int NUMBER_OF_MOTORS = 3;

PwmOut m3(M3_PWM);

Motor motor0(M0_PWM, M0_DIR1, M0_DIR2, M0_ENCA, M0_ENCB);
Motor motor1(M1_PWM, M1_DIR1, M1_DIR2, M1_ENCA, M1_ENCB);
Motor motor2(M2_PWM, M2_DIR1, M2_DIR2, M2_ENCA, M2_ENCB);
//Motor motor3(M3_PWM, M3_DIR1, M3_DIR2, M3_ENCA, M3_ENCB);

Motor * motors[NUMBER_OF_MOTORS] = {
    &motor0, &motor1, &motor2
};

DigitalOut servo(ISO_PWM5);

RFManager rfModule(COM1_TX, COM1_RX);

void parseCommand(char *command);

Ticker pidTicker;
unsigned int pidTickerCount = 0;
static const float PID_FREQ = 60;

char buf[BUF_SIZE];
int serialCount = 0;
bool serialData = false;

bool failSafeEnabled = true;
int failSafeCount = 0;
int failSafeLimit = 60;

Ticker softPWMTicker;
Timeout softPWMTimeout;
int currentServoPosition = 0;

void pidTick() {
    for (int i = 0; i < NUMBER_OF_MOTORS; i++) {
        motors[i]->pid();
    }

    if (pidTickerCount++ % 60 == 0) {
        led1 = !led1;
    }

    failSafeCount++;
    
    if (failSafeCount == failSafeLimit) {
        failSafeCount = 0;
        
        if (failSafeEnabled) {
            for (int i = 0; i < NUMBER_OF_MOTORS; ++i) {
                motors[i]->setSpeed(0);
            }

            m3.pulsewidth_us(100);
        }
    }
}

void softPWMEndPulse() {
    servo = 0;
}

void softPWMTick() {
    if(currentServoPosition <= 0) {
        servo = 0 ;
    } else {
        servo = 1;
        softPWMTimeout.attach_us(softPWMEndPulse, currentServoPosition);
    }
}

int main() {
    pc.baud(115200);
    
    pidTicker.attach(pidTick, 1 / PID_FREQ);

    m3.pulsewidth_us(100);
    //servo.pulsewidth_us(0);
    
    // TGY-180D (KC2462 controller) has problems with higher frequency PWM.
    // 50Hz works, but would like to use higher frequency for motors.
    // Software PWM seems to be good enough.    
    softPWMTicker.attach_us(softPWMTick, 20000);

    while (1) {
        if (rfModule.readable()) {
            serial.printf("<ref:%s>\n", rfModule.read());
        }

        rfModule.update();

        while (serial.readable()) {
            char c = serial.getc();
            
            buf[serialCount] = c;

            if (c == '\n') {
                parseCommand(buf);
                serialCount = 0;
                memset(buf, 0, BUF_SIZE);
            } else {
                serialCount++;
                
                if (serialCount == BUF_SIZE) {
                    serialCount = 0;
                }
            }
        }
    }
}

void parseCommand(char *buffer) {
    failSafeCount = 0;

    char *cmd = strtok(buffer, ":");

    // buffer == "sd:14:16:10:30"
    if (strncmp(cmd, "sd", 2) == 0) {
        for (int i = 0; i < NUMBER_OF_MOTORS; ++i) {
            motors[i]->setSpeed(atoi(strtok(NULL, ":")));
        }

        serial.printf("<gs:%d:%d:%d>\n",
            motors[0]->getSpeed(),
            motors[1]->getSpeed(),
            motors[2]->getSpeed()
         );
         
    } else if (strncmp(cmd, "d", 1) == 0) {
        int pulsewidth = atoi(buffer + 2);
        
        /*if (pulsewidth < 800) {
            pulsewidth = 800;
        } else if (pulsewidth > 2100) {
            pulsewidth = 2100;
        }*/
        
        m3.pulsewidth_us(pulsewidth);
             
    } else if (strncmp(cmd, "sv", 2) == 0) {
        currentServoPosition = atoi(buffer + 3);
        
        if (currentServoPosition < 700) {
            currentServoPosition = 0;
        } else if (currentServoPosition > 2300) {
            currentServoPosition = 2300;
        }
        
        //servo.pulsewidth_us(currentServoPosition);
             
    } else if (strncmp(cmd, "gs", 2) == 0) {
        serial.printf("<gs:%d:%d:%d>\n",
            motors[0]->getSpeed(),
            motors[1]->getSpeed(),
            motors[2]->getSpeed()
        );
    } else if (strncmp(cmd, "rf", 2) == 0) {
        rfModule.send(buffer + 3);
        
    } else if (strncmp(cmd, "fs", 1) == 0) {
        failSafeEnabled = buffer[3] != '0';
    }
}