IoTT Quad Control

Dependencies:   mbed

Fork of frdm_rgbled by Freescale

main.cpp

Committer:
johnmc
Date:
2015-10-06
Revision:
8:e282aa7c1fe9
Parent:
7:ad8295723268

File content as of revision 8:e282aa7c1fe9:

#include "mbed.h"

Serial pc(USBTX,USBRX);

PwmOut M1(PTD0);
PwmOut M2(PTD1);
PwmOut M3(PTD2);
PwmOut M4(PTD3);

//LED Outs
DigitalOut led_red(LED_RED);
DigitalOut led_green(LED_GREEN);

DigitalIn motion(D2);

int main()
{
    pc.printf("Loading...\n\r");
    pc.printf("%c\n\r",motion);
     M1.period(0.0025f);
    M2.period(0.0025f);
    M3.period(0.0025f);
    M4.period(0.0025f);
//Motor Off
    M1.write(0.0f);
    M2.write(0.0f);
    M3.write(0.0f);
    M4.write(0.00f);
    led_red = 1;
    led_green = 0;
    wait (2.0f);
//Wait for Motion         
    while (true) {
    led_red = 1;
    led_green = 0;
    pc.printf("Motors off; Waiting for motion...\n\r");
//Motor Off
    M1.write(0.0f);
    M2.write(0.0f);
    M3.write(0.0f);
    M4.write(0.0f);
    wait (.0025f);
          if(motion){
            pc.printf("Motors On");
            led_red = 0;
            led_green = 1;
            M1.write(0.5f);  //Anything less than 50% duty cycle did not spin my motors.
            M2.write(0.5f);
            M3.write(0.5f);
            M4.write(0.5f);
            wait (.0025f);             
            }
            //RAMP DOWN SEQUENCE to avoid ESC protection
            M1.write(0.25f);
            M2.write(0.25f);
            M3.write(0.25f);
            M4.write(0.25f);
            wait (.0025f);
            M1.write(0.15f);
            M2.write(0.15f);
            M3.write(0.15f);
            M4.write(0.15f);
            wait (.0025f);
            M1.write(0.0f);
            M2.write(0.0f);
            M3.write(0.0f);
            M4.write(0.0f);
            wait (.0025f);
          }
    }