Serpentine / Mbed 2 deprecated serpentine_snakecontroller_final

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.

Dependencies:   MMA8451Q TSI mbed

Fork of snakecontroller by Serpentine

main.cpp@5:97c02241511c, 2016-06-09 (annotated)

Committer:
danlock10y
Date:
Thu Jun 09 10:36:19 2016 +0000
Revision:
5:97c02241511c
Parent:
4:1961b60aa8fb
Child:
6:44bac63d4d66
Snake Controller

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
danlock10y 1:6dd9eca697db 8 MMA8451Q acc(PTE25, PTE24, MMA8451_I2C_ADDRESS); //Setup accelerometer
danlock10y 1:6dd9eca697db 9 DigitalOut lefthigh(PTC11); //Test LED for left
danlock10y 1:6dd9eca697db 10 DigitalOut leftlow(PTC9);
danlock10y 1:6dd9eca697db 11 DigitalOut righthigh(PTC5); //Test LED for right
danlock10y 1:6dd9eca697db 12 DigitalOut rightlow(PTA4);
danlock10y 1:6dd9eca697db 13 DigitalOut forwardhigh(PTC16); //Test LED for forward
danlock10y 1:6dd9eca697db 14 DigitalOut forwardlow(PTD0);
danlock10y 1:6dd9eca697db 15 DigitalOut backwardhigh(PTA17); //Test LED for backward
danlock10y 1:6dd9eca697db 16 DigitalOut backwardlow(PTD1);
danlock10y 1:6dd9eca697db 17
danlock10y 1:6dd9eca697db 18 float accX=acc.getAccX(); //Measure acceleration in X direction
danlock10y 1:6dd9eca697db 19 float accY=acc.getAccY(); //Measure acceleration in Y direction
danlock10y 1:6dd9eca697db 20
danlock10y 1:6dd9eca697db 21 int Direction; //2 bit number to describe direction 0 forward assigned CW
danlock10y 1:6dd9eca697db 22
danlock10y 1:6dd9eca697db 23 bool right; //Right or left? right = 1
danlock10y 1:6dd9eca697db 24 bool forward; //Forward or Backward? forward = 1
danlock10y 1:6dd9eca697db 25
pemb4660 0:c5a6d47904dc 26 int main()
pemb4660 0:c5a6d47904dc 27 {
danlock10y 1:6dd9eca697db 28 while (1)
danlock10y 1:6dd9eca697db 29 {
danlock10y 3:768cca7d53fe 30
danlock10y 3:768cca7d53fe 31 float accX=acc.getAccX(); //Measure acceleration in X direction
danlock10y 3:768cca7d53fe 32 float accY=acc.getAccY(); //Measure acceleration in Y direction
danlock10y 3:768cca7d53fe 33
danlock10y 1:6dd9eca697db 34 if((accX <= 0.1f) && (accX >= -0.1f))
danlock10y 1:6dd9eca697db 35 {
danlock10y 3:768cca7d53fe 36 if (right == false)
danlock10y 1:6dd9eca697db 37 {
danlock10y 1:6dd9eca697db 38 righthigh = 1;
danlock10y 1:6dd9eca697db 39 rightlow = 0;
danlock10y 1:6dd9eca697db 40 } //endif
danlock10y 1:6dd9eca697db 41 else
danlock10y 1:6dd9eca697db 42 {
danlock10y 1:6dd9eca697db 43 lefthigh = 1;
danlock10y 1:6dd9eca697db 44 leftlow = 0;
danlock10y 1:6dd9eca697db 45 }//endelse
danlock10y 1:6dd9eca697db 46 }//endif
danlock10y 3:768cca7d53fe 47
danlock10y 1:6dd9eca697db 48 if (accX > 0.1)
danlock10y 1:6dd9eca697db 49 {
danlock10y 1:6dd9eca697db 50 lefthigh = 0;
danlock10y 1:6dd9eca697db 51 leftlow = 0;
danlock10y 1:6dd9eca697db 52 righthigh = 1;
danlock10y 1:6dd9eca697db 53 rightlow = 0;
danlock10y 1:6dd9eca697db 54 forwardhigh = 0;
danlock10y 1:6dd9eca697db 55 forwardlow = 0;
danlock10y 1:6dd9eca697db 56 backwardhigh = 0;
danlock10y 1:6dd9eca697db 57 backwardlow = 0;
danlock10y 3:768cca7d53fe 58 right = false;
danlock10y 3:768cca7d53fe 59 printf("left \r\n");
danlock10y 1:6dd9eca697db 60 }//endif
pemb4660 0:c5a6d47904dc 61
danlock10y 1:6dd9eca697db 62 if (accX < -0.1)
danlock10y 1:6dd9eca697db 63 {
danlock10y 1:6dd9eca697db 64 lefthigh = 1;
danlock10y 1:6dd9eca697db 65 leftlow = 0;
danlock10y 1:6dd9eca697db 66 righthigh = 0;
danlock10y 1:6dd9eca697db 67 rightlow = 0;
danlock10y 1:6dd9eca697db 68 forwardhigh = 0;
danlock10y 1:6dd9eca697db 69 forwardlow = 0;
danlock10y 1:6dd9eca697db 70 backwardhigh = 0;
danlock10y 1:6dd9eca697db 71 backwardlow = 0;
danlock10y 3:768cca7d53fe 72 right = true;
danlock10y 3:768cca7d53fe 73 printf("right \r\n");
danlock10y 1:6dd9eca697db 74 }//endif
danlock10y 3:768cca7d53fe 75 wait(0.1);
danlock10y 1:6dd9eca697db 76
danlock10y 3:768cca7d53fe 77 if((accY <= 0.1) && (accY >= -0.1))
danlock10y 1:6dd9eca697db 78 {
danlock10y 3:768cca7d53fe 79 if (forward == false)
danlock10y 1:6dd9eca697db 80 {
pemb4660 0:c5a6d47904dc 81 forwardhigh = 1;
danlock10y 1:6dd9eca697db 82 forwardlow = 0;
danlock10y 1:6dd9eca697db 83 }//endif
danlock10y 1:6dd9eca697db 84 else
danlock10y 1:6dd9eca697db 85 {
pemb4660 0:c5a6d47904dc 86 backwardhigh = 1;
danlock10y 1:6dd9eca697db 87 backwardlow = 0;
danlock10y 1:6dd9eca697db 88 }//endelse
danlock10y 1:6dd9eca697db 89 }//endif
pemb4660 0:c5a6d47904dc 90
danlock10y 3:768cca7d53fe 91 if (accY > 0.1)
danlock10y 1:6dd9eca697db 92 {
danlock10y 1:6dd9eca697db 93 lefthigh = 0;
danlock10y 1:6dd9eca697db 94 leftlow = 0;
danlock10y 1:6dd9eca697db 95 righthigh = 0;
danlock10y 1:6dd9eca697db 96 rightlow = 0;
danlock10y 1:6dd9eca697db 97 forwardhigh = 1;
danlock10y 1:6dd9eca697db 98 forwardlow = 0;
danlock10y 1:6dd9eca697db 99 backwardhigh = 0;
danlock10y 1:6dd9eca697db 100 backwardlow = 0;
danlock10y 3:768cca7d53fe 101 forward = false;
danlock10y 3:768cca7d53fe 102 printf("back \r\n");
danlock10y 1:6dd9eca697db 103 }//endif
pemb4660 0:c5a6d47904dc 104
danlock10y 3:768cca7d53fe 105 if (accY < -0.1)
danlock10y 1:6dd9eca697db 106 {
danlock10y 1:6dd9eca697db 107 lefthigh = 0;
danlock10y 1:6dd9eca697db 108 leftlow = 0;
danlock10y 1:6dd9eca697db 109 righthigh = 0;
danlock10y 1:6dd9eca697db 110 rightlow = 0;
danlock10y 1:6dd9eca697db 111 forwardhigh = 0;
danlock10y 1:6dd9eca697db 112 forwardlow = 0;
danlock10y 1:6dd9eca697db 113 backwardhigh = 1;
danlock10y 1:6dd9eca697db 114 backwardlow = 0;
danlock10y 3:768cca7d53fe 115 forward = true;
danlock10y 3:768cca7d53fe 116 printf("for \r\n");
danlock10y 1:6dd9eca697db 117 }//endif
danlock10y 3:768cca7d53fe 118
danlock10y 3:768cca7d53fe 119 wait(0.1);
danlock10y 3:768cca7d53fe 120
danlock10y 4:1961b60aa8fb 121 if(abs(accY) > abs(accX))
danlock10y 3:768cca7d53fe 122 {
danlock10y 4:1961b60aa8fb 123 if(forward == true)
danlock10y 4:1961b60aa8fb 124 {
danlock10y 4:1961b60aa8fb 125 Direction = 0;
danlock10y 4:1961b60aa8fb 126 }//endif
danlock10y 4:1961b60aa8fb 127 else
danlock10y 3:768cca7d53fe 128 {
danlock10y 4:1961b60aa8fb 129 Direction = 2;
danlock10y 4:1961b60aa8fb 130 }//endelse
danlock10y 4:1961b60aa8fb 131 }//endif
danlock10y 4:1961b60aa8fb 132 else
danlock10y 4:1961b60aa8fb 133 {
danlock10y 4:1961b60aa8fb 134 if(right == true)
danlock10y 4:1961b60aa8fb 135 {
danlock10y 4:1961b60aa8fb 136 Direction = 1;
danlock10y 4:1961b60aa8fb 137 }//endif
danlock10y 4:1961b60aa8fb 138 else
danlock10y 4:1961b60aa8fb 139 {
danlock10y 4:1961b60aa8fb 140 Direction = 3;
danlock10y 4:1961b60aa8fb 141 }//endelse
danlock10y 4:1961b60aa8fb 142 }//endelse
danlock10y 4:1961b60aa8fb 143
danlock10y 4:1961b60aa8fb 144 printf("Direction = %d \r\n", Direction);
danlock10y 4:1961b60aa8fb 145
danlock10y 1:6dd9eca697db 146 }//endwhile
danlock10y 1:6dd9eca697db 147 }//endmain