Serpentine / Mbed 2 deprecated snakecontroller

Controller for snake

Dependencies:   MMA8451Q TSI mbed

Fork of foodcontroller3 by Serpentine

main.cpp@0:c5a6d47904dc, 2016-06-08 (annotated)

Committer:
pemb4660
Date:
Wed Jun 08 16:04:32 2016 +0000
Revision:
0:c5a6d47904dc
Child:
1:6dd9eca697db
hyi8ndsfop;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pemb4660 0:c5a6d47904dc 1 #include "mbed.h"
pemb4660 0:c5a6d47904dc 2 #include "TSISensor.h"
pemb4660 0:c5a6d47904dc 3 #include "MMA8451Q.h"
pemb4660 0:c5a6d47904dc 4
pemb4660 0:c5a6d47904dc 5 #define MMA8451_I2C_ADDRESS (0x1d<<1)
pemb4660 0:c5a6d47904dc 6 Serial pc(USBTX, USBRX); // tx, rx
pemb4660 0:c5a6d47904dc 7
pemb4660 0:c5a6d47904dc 8 int main()
pemb4660 0:c5a6d47904dc 9 {
pemb4660 0:c5a6d47904dc 10 PwmOut gled(LED_GREEN);
pemb4660 0:c5a6d47904dc 11 TSISensor tsi;
pemb4660 0:c5a6d47904dc 12 MMA8451Q acc(PTE25, PTE24, MMA8451_I2C_ADDRESS);
pemb4660 0:c5a6d47904dc 13 DigitalOut lefthigh(PTC11);
pemb4660 0:c5a6d47904dc 14 DigitalOut leftlow(PTC9);
pemb4660 0:c5a6d47904dc 15 DigitalOut righthigh(PTC6);
pemb4660 0:c5a6d47904dc 16 DigitalOut rightlow(PTA5);
pemb4660 0:c5a6d47904dc 17 DigitalOut forwardhigh(PTC16);
pemb4660 0:c5a6d47904dc 18 DigitalOut forwardlow(PTD0);
pemb4660 0:c5a6d47904dc 19 DigitalOut backwardhigh(PTA17);
pemb4660 0:c5a6d47904dc 20 DigitalOut backwardlow(PTD1);
pemb4660 0:c5a6d47904dc 21
pemb4660 0:c5a6d47904dc 22
pemb4660 0:c5a6d47904dc 23 bool right;
pemb4660 0:c5a6d47904dc 24 bool forward;
pemb4660 0:c5a6d47904dc 25
pemb4660 0:c5a6d47904dc 26
pemb4660 0:c5a6d47904dc 27 while (1) {
pemb4660 0:c5a6d47904dc 28
pemb4660 0:c5a6d47904dc 29 wait(0.2);
pemb4660 0:c5a6d47904dc 30 float accX=acc.getAccX();
pemb4660 0:c5a6d47904dc 31 printf("x=%0.3f\r\n",accX);
pemb4660 0:c5a6d47904dc 32 if (tsi.readPercentage() != 0) {
pemb4660 0:c5a6d47904dc 33 gled = 1.0 - tsi.readPercentage();
pemb4660 0:c5a6d47904dc 34 }
pemb4660 0:c5a6d47904dc 35
pemb4660 0:c5a6d47904dc 36 if((accX <= 0.1f) && (accX >= -0.1f)) {
pemb4660 0:c5a6d47904dc 37 printf("A \r\n");
pemb4660 0:c5a6d47904dc 38 if (right == true) {
pemb4660 0:c5a6d47904dc 39 printf("B \r\n");
pemb4660 0:c5a6d47904dc 40 righthigh = 0;
pemb4660 0:c5a6d47904dc 41 rightlow = 1;
pemb4660 0:c5a6d47904dc 42 } else {
pemb4660 0:c5a6d47904dc 43 printf("C \r\n");
pemb4660 0:c5a6d47904dc 44 lefthigh = 0;
pemb4660 0:c5a6d47904dc 45 leftlow = 1;
pemb4660 0:c5a6d47904dc 46 }
pemb4660 0:c5a6d47904dc 47 //}
pemb4660 0:c5a6d47904dc 48
pemb4660 0:c5a6d47904dc 49 if (accX > 0.1) {
pemb4660 0:c5a6d47904dc 50 printf("D \r\n");
pemb4660 0:c5a6d47904dc 51 lefthigh = 1;
pemb4660 0:c5a6d47904dc 52 leftlow = 1;
pemb4660 0:c5a6d47904dc 53 righthigh = 0;
pemb4660 0:c5a6d47904dc 54 rightlow = 1;
pemb4660 0:c5a6d47904dc 55 forwardhigh = 1;
pemb4660 0:c5a6d47904dc 56 forwardlow = 1;
pemb4660 0:c5a6d47904dc 57 backwardhigh = 1;
pemb4660 0:c5a6d47904dc 58 backwardlow = 1;
pemb4660 0:c5a6d47904dc 59 right = true;
pemb4660 0:c5a6d47904dc 60 }
pemb4660 0:c5a6d47904dc 61
pemb4660 0:c5a6d47904dc 62 if (accX < -0.1) {
pemb4660 0:c5a6d47904dc 63 printf("E \r\n");
pemb4660 0:c5a6d47904dc 64 lefthigh = 0;
pemb4660 0:c5a6d47904dc 65 leftlow = 1;
pemb4660 0:c5a6d47904dc 66 righthigh = 1;
pemb4660 0:c5a6d47904dc 67 rightlow = 1;
pemb4660 0:c5a6d47904dc 68 forwardhigh = 1;
pemb4660 0:c5a6d47904dc 69 forwardlow = 1;
pemb4660 0:c5a6d47904dc 70 backwardhigh = 1;
pemb4660 0:c5a6d47904dc 71 backwardlow = 1;
pemb4660 0:c5a6d47904dc 72 right = false;
pemb4660 0:c5a6d47904dc 73 }
pemb4660 0:c5a6d47904dc 74 wait(0.1);
pemb4660 0:c5a6d47904dc 75 }
pemb4660 0:c5a6d47904dc 76
pemb4660 0:c5a6d47904dc 77
pemb4660 0:c5a6d47904dc 78 if((acc.getAccY() <= 0.1) && (acc.getAccY() >= -0.1)) {
pemb4660 0:c5a6d47904dc 79 if (forward == true) {
pemb4660 0:c5a6d47904dc 80 forwardhigh = 0;
pemb4660 0:c5a6d47904dc 81 forwardlow = 1;
pemb4660 0:c5a6d47904dc 82 } else
pemb4660 0:c5a6d47904dc 83 backwardhigh = 0;
pemb4660 0:c5a6d47904dc 84 backwardlow = 1;
pemb4660 0:c5a6d47904dc 85 }
pemb4660 0:c5a6d47904dc 86
pemb4660 0:c5a6d47904dc 87 if (acc.getAccY() > 0.1) {
pemb4660 0:c5a6d47904dc 88 lefthigh = 1;
pemb4660 0:c5a6d47904dc 89 leftlow = 1;
pemb4660 0:c5a6d47904dc 90 righthigh = 1;
pemb4660 0:c5a6d47904dc 91 rightlow = 1;
pemb4660 0:c5a6d47904dc 92 forwardhigh = 0;
pemb4660 0:c5a6d47904dc 93 forwardlow = 1;
pemb4660 0:c5a6d47904dc 94 backwardhigh = 1;
pemb4660 0:c5a6d47904dc 95 backwardlow = 1;
pemb4660 0:c5a6d47904dc 96 forward = true;
pemb4660 0:c5a6d47904dc 97 }
pemb4660 0:c5a6d47904dc 98
pemb4660 0:c5a6d47904dc 99 if (acc.getAccY() < -0.1) {
pemb4660 0:c5a6d47904dc 100 lefthigh = 1;
pemb4660 0:c5a6d47904dc 101 leftlow = 1;
pemb4660 0:c5a6d47904dc 102 righthigh = 1;
pemb4660 0:c5a6d47904dc 103 rightlow = 1;
pemb4660 0:c5a6d47904dc 104 forwardhigh = 1;
pemb4660 0:c5a6d47904dc 105 forwardlow = 1;
pemb4660 0:c5a6d47904dc 106 backwardhigh = 0;
pemb4660 0:c5a6d47904dc 107 backwardlow = 1;
pemb4660 0:c5a6d47904dc 108 forward = false;
pemb4660 0:c5a6d47904dc 109 }
pemb4660 0:c5a6d47904dc 110 wait(0.1);
pemb4660 0:c5a6d47904dc 111 }
pemb4660 0:c5a6d47904dc 112 }