Controller for snake

Dependencies:   MMA8451Q TSI mbed

Fork of foodcontroller3 by Serpentine

main.cpp

Committer:
pemb4660
Date:
2016-06-08
Revision:
0:c5a6d47904dc
Child:
1:6dd9eca697db

File content as of revision 0:c5a6d47904dc:

#include "mbed.h"
#include "TSISensor.h"
#include "MMA8451Q.h"

#define MMA8451_I2C_ADDRESS (0x1d<<1)
Serial pc(USBTX, USBRX); // tx, rx

int main()
{
    PwmOut gled(LED_GREEN);
    TSISensor tsi;
    MMA8451Q acc(PTE25, PTE24, MMA8451_I2C_ADDRESS);
    DigitalOut lefthigh(PTC11);
    DigitalOut leftlow(PTC9);
    DigitalOut righthigh(PTC6);
    DigitalOut rightlow(PTA5);
    DigitalOut forwardhigh(PTC16);
    DigitalOut forwardlow(PTD0);
    DigitalOut backwardhigh(PTA17);
    DigitalOut backwardlow(PTD1);


    bool right;
    bool forward;


    while (1) {

        wait(0.2);
        float accX=acc.getAccX();
        printf("x=%0.3f\r\n",accX);
        if (tsi.readPercentage() != 0) {
            gled = 1.0 - tsi.readPercentage();
        }

        if((accX <= 0.1f) && (accX >= -0.1f)) {
            printf("A \r\n");
            if (right == true) {
                printf("B \r\n");
                righthigh = 0;
                rightlow = 1;
            } else {
                printf("C \r\n");
                lefthigh = 0;
                leftlow = 1;
            }
            //}

            if (accX > 0.1) {
                printf("D \r\n");
                lefthigh = 1;
                leftlow = 1;
                righthigh = 0;
                rightlow = 1;
                forwardhigh = 1;
                forwardlow = 1;
                backwardhigh = 1;
                backwardlow = 1;
                right = true;
            }

            if (accX < -0.1) {
                printf("E \r\n");
                lefthigh = 0;
                leftlow = 1;
                righthigh = 1;
                rightlow = 1;
                forwardhigh = 1;
                forwardlow = 1;
                backwardhigh = 1;
                backwardlow = 1;
                right = false;
            }
            wait(0.1);
        }


        if((acc.getAccY() <= 0.1) && (acc.getAccY() >= -0.1)) {
            if (forward == true) {
                forwardhigh = 0;
                forwardlow = 1;
            } else
                backwardhigh = 0;
            backwardlow = 1;
        }

        if (acc.getAccY() > 0.1) {
            lefthigh = 1;
            leftlow = 1;
            righthigh = 1;
            rightlow = 1;
            forwardhigh = 0;
            forwardlow = 1;
            backwardhigh = 1;
            backwardlow = 1;
            forward = true;
        }

        if (acc.getAccY() < -0.1) {
            lefthigh = 1;
            leftlow = 1;
            righthigh = 1;
            rightlow = 1;
            forwardhigh = 1;
            forwardlow = 1;
            backwardhigh = 0;
            backwardlow = 1;
            forward = false;
        }
        wait(0.1);
    }
}