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@9:7628a5b8b6f2, 2016-06-09 (annotated)

Committer:
danlock10y
Date:
Thu Jun 09 18:36:04 2016 +0000
Revision:
9:7628a5b8b6f2
Parent:
8:93190b81ceca 
Fully commented; ; Functions with 2 axis tilting

Who changed what in which revision?

UserRevisionLine numberNew contents of line
danlock10y 8:93190b81ceca 1 /**
danlock10y 8:93190b81ceca 2 Snake Controller
danlock10y 8:93190b81ceca 3 snakeController.cpp
danlock10y 8:93190b81ceca 4 Purpose: This function runs on a FRDM KL25Z. It uses the accelerometer to measure the tilt of the board.
danlock10y 8:93190b81ceca 5 It outputs a 2 bit number that describes the direction. The board is also fitted with four LEDS
danlock10y 8:93190b81ceca 6 oriented in the following configuration: o8o to provide feedback to the user.
danlock10y 8:93190b81ceca 7 @author Daniel Lock
danlock10y 8:93190b81ceca 8 @author Jamie Gnodde
danlock10y 8:93190b81ceca 9 @version
danlock10y 8:93190b81ceca 10 */
danlock10y 8:93190b81ceca 11
danlock10y 8:93190b81ceca 12
pemb4660 0:c5a6d47904dc 13 #include "mbed.h"
pemb4660 0:c5a6d47904dc 14 #include "TSISensor.h"
pemb4660 0:c5a6d47904dc 15 #include "MMA8451Q.h"
pemb4660 0:c5a6d47904dc 16
pemb4660 0:c5a6d47904dc 17 #define MMA8451_I2C_ADDRESS (0x1d<<1)
danlock10y 8:93190b81ceca 18 Serial pc(USBTX, USBRX); // tx, rx - Set up USB serial to read back values for testing
pemb4660 0:c5a6d47904dc 19
danlock10y 1:6dd9eca697db 20 MMA8451Q acc(PTE25, PTE24, MMA8451_I2C_ADDRESS); //Setup accelerometer
danlock10y 6:44bac63d4d66 21 DigitalOut lefthigh(PTC7); //Test LED for left
danlock10y 6:44bac63d4d66 22 DigitalOut leftlow(PTA1);
danlock10y 6:44bac63d4d66 23 DigitalOut righthigh(PTC4); //Test LED for right
danlock10y 6:44bac63d4d66 24 DigitalOut rightlow(PTA12);
danlock10y 6:44bac63d4d66 25 DigitalOut forwardhigh(PTC0); //Test LED for forward
danlock10y 6:44bac63d4d66 26 DigitalOut forwardlow(PTC3);
danlock10y 6:44bac63d4d66 27 DigitalOut backwardhigh(PTA2); //Test LED for backward
danlock10y 6:44bac63d4d66 28 DigitalOut backwardlow(PTD4);
danlock10y 1:6dd9eca697db 29
danlock10y 1:6dd9eca697db 30 int Direction; //2 bit number to describe direction 0 forward assigned CW
danlock10y 1:6dd9eca697db 31
danlock10y 1:6dd9eca697db 32 bool right; //Right or left? right = 1
danlock10y 1:6dd9eca697db 33 bool forward; //Forward or Backward? forward = 1
danlock10y 1:6dd9eca697db 34
pemb4660 0:c5a6d47904dc 35 int main()
pemb4660 0:c5a6d47904dc 36 {
danlock10y 1:6dd9eca697db 37 while (1)
danlock10y 1:6dd9eca697db 38 {
danlock10y 3:768cca7d53fe 39
danlock10y 3:768cca7d53fe 40 float accX=acc.getAccX(); //Measure acceleration in X direction
danlock10y 3:768cca7d53fe 41 float accY=acc.getAccY(); //Measure acceleration in Y direction
danlock10y 8:93190b81ceca 42
danlock10y 8:93190b81ceca 43 //Establish whether the board is tilted left or right
danlock10y 1:6dd9eca697db 44 if (accX > 0.1)
danlock10y 1:6dd9eca697db 45 {
danlock10y 3:768cca7d53fe 46 right = false;
danlock10y 6:44bac63d4d66 47 //printf("left \r\n");
danlock10y 1:6dd9eca697db 48 }//endif
pemb4660 0:c5a6d47904dc 49
danlock10y 1:6dd9eca697db 50 if (accX < -0.1)
danlock10y 1:6dd9eca697db 51 {
danlock10y 3:768cca7d53fe 52 right = true;
danlock10y 6:44bac63d4d66 53 //printf("right \r\n");
danlock10y 1:6dd9eca697db 54 }//endif
danlock10y 8:93190b81ceca 55
danlock10y 8:93190b81ceca 56 wait(0.1); //Not sure if this is left over from debugging
danlock10y 1:6dd9eca697db 57
danlock10y 8:93190b81ceca 58 //Establish whether the board is tilted front or back
danlock10y 3:768cca7d53fe 59 if (accY > 0.1)
danlock10y 1:6dd9eca697db 60 {
danlock10y 3:768cca7d53fe 61 forward = false;
danlock10y 6:44bac63d4d66 62 //printf("back \r\n");
danlock10y 1:6dd9eca697db 63 }//endif
pemb4660 0:c5a6d47904dc 64
danlock10y 3:768cca7d53fe 65 if (accY < -0.1)
danlock10y 1:6dd9eca697db 66 {
danlock10y 3:768cca7d53fe 67 forward = true;
danlock10y 6:44bac63d4d66 68 //printf("for \r\n");
danlock10y 1:6dd9eca697db 69 }//endif
danlock10y 3:768cca7d53fe 70
danlock10y 8:93190b81ceca 71 wait(0.1); //Not sure if this is left over from debugging
danlock10y 3:768cca7d53fe 72
danlock10y 8:93190b81ceca 73 //Establish the main axis of tilting so that the control outputs one direction
danlock10y 4:1961b60aa8fb 74 if(abs(accY) > abs(accX))
danlock10y 3:768cca7d53fe 75 {
danlock10y 4:1961b60aa8fb 76 if(forward == true)
danlock10y 4:1961b60aa8fb 77 {
danlock10y 9:7628a5b8b6f2 78 Direction = 0; //Direction variable is a two bit number 0-3 0 is forward
danlock10y 6:44bac63d4d66 79
danlock10y 9:7628a5b8b6f2 80 lefthigh = 0; //Light up the forward LED, make sure all others are off
danlock10y 6:44bac63d4d66 81 leftlow = 0;
danlock10y 6:44bac63d4d66 82 righthigh = 0;
danlock10y 6:44bac63d4d66 83 rightlow = 0;
danlock10y 6:44bac63d4d66 84 forwardhigh = 1;
danlock10y 6:44bac63d4d66 85 forwardlow = 0;
danlock10y 6:44bac63d4d66 86 backwardhigh = 0;
danlock10y 6:44bac63d4d66 87 backwardlow = 0;
danlock10y 6:44bac63d4d66 88
danlock10y 4:1961b60aa8fb 89 }//endif
danlock10y 4:1961b60aa8fb 90 else
danlock10y 3:768cca7d53fe 91 {
danlock10y 9:7628a5b8b6f2 92 Direction = 2; //Direction variable is a two bit number 0-3 2 is backward
danlock10y 6:44bac63d4d66 93
danlock10y 9:7628a5b8b6f2 94 lefthigh = 0; //Light up the backward LED, make sure all others are off
danlock10y 6:44bac63d4d66 95 leftlow = 0;
danlock10y 6:44bac63d4d66 96 righthigh = 0;
danlock10y 6:44bac63d4d66 97 rightlow = 0;
danlock10y 6:44bac63d4d66 98 forwardhigh = 0;
danlock10y 6:44bac63d4d66 99 forwardlow = 0;
danlock10y 6:44bac63d4d66 100 backwardhigh = 1;
danlock10y 6:44bac63d4d66 101 backwardlow = 0;
danlock10y 6:44bac63d4d66 102
danlock10y 4:1961b60aa8fb 103 }//endelse
danlock10y 4:1961b60aa8fb 104 }//endif
danlock10y 4:1961b60aa8fb 105 else
danlock10y 4:1961b60aa8fb 106 {
danlock10y 4:1961b60aa8fb 107 if(right == true)
danlock10y 4:1961b60aa8fb 108 {
danlock10y 9:7628a5b8b6f2 109 Direction = 1; //Direction variable is a two bit number 0-3 1 is right
danlock10y 6:44bac63d4d66 110
danlock10y 9:7628a5b8b6f2 111 lefthigh = 0; //Light up the right LED, make sure all others are off
danlock10y 6:44bac63d4d66 112 leftlow = 0;
danlock10y 6:44bac63d4d66 113 righthigh = 1;
danlock10y 6:44bac63d4d66 114 rightlow = 0;
danlock10y 6:44bac63d4d66 115 forwardhigh = 0;
danlock10y 6:44bac63d4d66 116 forwardlow = 0;
danlock10y 6:44bac63d4d66 117 backwardhigh = 0;
danlock10y 6:44bac63d4d66 118 backwardlow = 0;
danlock10y 6:44bac63d4d66 119
danlock10y 4:1961b60aa8fb 120 }//endif
danlock10y 4:1961b60aa8fb 121 else
danlock10y 4:1961b60aa8fb 122 {
danlock10y 9:7628a5b8b6f2 123 Direction = 3; //Direction variable is a two bit number 0-3 3 is left
danlock10y 6:44bac63d4d66 124
danlock10y 9:7628a5b8b6f2 125 lefthigh = 1; //Light up the left LED, make sure all others are off
danlock10y 6:44bac63d4d66 126 leftlow = 0;
danlock10y 6:44bac63d4d66 127 righthigh = 0;
danlock10y 6:44bac63d4d66 128 rightlow = 0;
danlock10y 6:44bac63d4d66 129 forwardhigh = 0;
danlock10y 6:44bac63d4d66 130 forwardlow = 0;
danlock10y 6:44bac63d4d66 131 backwardhigh = 0;
danlock10y 6:44bac63d4d66 132 backwardlow = 0;
danlock10y 6:44bac63d4d66 133
danlock10y 4:1961b60aa8fb 134 }//endelse
danlock10y 4:1961b60aa8fb 135 }//endelse
danlock10y 4:1961b60aa8fb 136
danlock10y 9:7628a5b8b6f2 137 printf("Direction = %d \r\n", Direction); //Print the value of Direction to the serial for debugging
danlock10y 4:1961b60aa8fb 138
danlock10y 1:6dd9eca697db 139 }//endwhile
danlock10y 1:6dd9eca697db 140 }//endmain