Raj Patel
Final Version.
Dependencies: Motor Servo mbed
Diff: main.cpp
- Revision:
- 0:8eae572f6d20
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Mar 11 18:59:21 2016 +0000
@@ -0,0 +1,264 @@
+/*
+
+@Authors: Raj Patel, David Ehrlich
+@Project: Bluetooth Controlled Robot with Dual Servo Sensor Actuated by Mobile Phone Accelerometer
+@Date: 03/09/2016
+
+*/
+
+//include necessary header files for functionality. Specifically, need servo handlers.
+
+#include "mbed.h"
+#include "Servo.h"
+
+Serial bluemod(p9,p10); // Serial hookup for Bluetooth module
+PwmOut left_led(LED3); // PWM output for left robot direction signal
+PwmOut right_led(LED4); // PWM output for right robot direction signal
+PwmOut forward_led(LED1); // PWM output for forward robot direction signal
+PwmOut reverse_led(LED2); // PWM output for reverse robot direction signal
+Servo myservolr(p21); // Signal for left/right servo
+Servo myservoud(p22); // Signal for up/down servo
+AnalogIn sensor(p15); // Signal for Sharp IR Sensor
+
+ PwmOut speedA(p23); // PWM Speed pin for Motor A
+ DigitalOut fwdA(p17); // Digital out signal for forward direction for Motor A
+ DigitalOut revA(p18); // Digital out signal for reverse direction for Motor A
+
+ PwmOut speedB(p24); // PWM Speed pin for Motor B
+ DigitalOut fwdB(p20); // Digital out signal for forward direction for Motor B
+ DigitalOut revB(p19); // Digital out signal for reverse direction for Motor B
+
+
+ volatile char exc; // Variable to keep track of ! when seen in BT stream
+ volatile char letter; // Varibale to keep track of character after ! in BT stream (A for accelerometer, B for buttons)
+
+//C union can convert 4 chars to a float - puts them in same location in memory
+//trick to pack the 4 bytes from Bluetooth serial port back into a 32-bit float
+union f_or_char {
+ float f;
+ char c[4];
+};
+
+int main()
+{
+ char bchecksum=0; // Checksum variable
+ char temp=0; // Temporary variable to hold BT stream data
+ union f_or_char x,y,z; // Templatized containers for x, y, and z accelerometer readings
+ float adjlr = 0; // Servo position variable for left/right servo
+ float adjud = 0; // Servo position variable for up/down servo
+ while(1) {
+ float sensorValue = sensor.read();
+ //printf("Sensor: %2.2f\n\r", sensorValue);
+ bchecksum=0;
+ exc = bluemod.getc(); // Get the first character in the bluetooth packet
+ if (exc=='!'){ // First character was ! meaning it could be !A or !B
+ letter = bluemod.getc(); // Get the next character in the bluetooth packet after the !
+ switch (letter) {
+ case 'A':{ // If the next character is A, it is an Accelerometer data packet
+ for (int i=0; i<4; i++) {
+ temp = bluemod.getc();
+ x.c[i] = temp; // Get x direction Accelerometer data
+ bchecksum = bchecksum + temp;
+ }
+ for (int i=0; i<4; i++) {
+ temp = bluemod.getc();
+ y.c[i] = temp; // Get y direction Accelerometer data
+ bchecksum = bchecksum + temp;
+ }
+ for (int i=0; i<4; i++) {
+ temp = bluemod.getc();
+ z.c[i] = temp; // Get z direction Accelerometer data
+ bchecksum = bchecksum + temp;
+ }
+ if (bluemod.getc()==char(~('!' + 'A' + bchecksum))) { //checksum OK? Then proceed.
+
+ // Both segments of the below code in this 'if' statement handle actuating the dual servo setup
+
+ // Below code segment handles left/right movement
+ if (y.f < -0.05 && y.f >= -0.10){
+ adjlr += 0.01; // If the y direction shows a small negative value, move left slowly
+ }
+ if (y.f < -0.10 && y.f >= -0.25){
+ adjlr += 0.05; // If the y direction shows a medium negative value, move left faster
+ }
+ if (y.f < -0.25){
+ adjlr += 0.09; // If the y direction shows a large negative value, move left fastest
+ }
+ if (y.f > 0.05 && y.f <= 0.10){
+ adjlr += -0.01; // If the y direction shows a small positive value, move right slowly
+ }
+ if (y.f > 0.10 && y.f <= 0.25){
+ adjlr += -0.05; // If the y direction shows a medium positive value, move right faster
+ }
+ if (y.f > 0.25){
+ adjlr += -0.09; // If the y direction shows a large positive value, move right fastest
+ }
+ if (adjlr < 0){
+ adjlr = 0; // If the value becomes less than 0, cap at 0
+ }else{
+ if (adjlr > 1){
+ adjlr = 1; // If the value becomes greater than 1, cap at 1
+ }
+ }
+
+ // Below code handles up/down movement
+ if (x.f < -0.05 && x.f >= -0.2){
+ adjud += -0.01; // If the x direction shows a small negative value, move up slowly
+ }
+ if (x.f < -0.2 && x.f >= -0.40){
+ adjud += -0.05; // If the x direction shows a medium negative value, move up faster
+ }
+ if (x.f < -0.4){
+ adjud += -0.09; // If the x direction shows a large negative value, move up fastest
+ }
+ if (x.f > 0.05 && x.f <= 0.2){
+ adjud += 0.01; // If the x direction shows a small positive value, move down slowly
+ }
+ if (x.f > 0.2 && x.f <= 0.40){
+ adjud += 0.05; // If the x direction shows a medium positive value, move down faster
+ }
+ if (x.f > 0.40){
+ adjud += 0.09; // If the x direction shows a large positive value, move down fastest
+ }
+ if (adjud < 0){
+ adjud = 0; // If the value becomes less than 0, cap at 0
+ }else{
+ if (adjud > 1){
+ adjud = 1; // If the value becomes greater than 1, cap at 1
+ }
+ }
+
+ myservolr = adjlr; // Send adjusted left/right signal to left/right servo
+ myservoud = adjud; // Send adjusted up/down signal to up/down servo
+
+
+ }
+ break;
+ }
+ char bnum; //Initialize variables to use below
+ char bhit; //Initialize variables to use below
+
+ case 'B':{ // If the next character is B, it is a Button data packet
+
+ bnum = bluemod.getc(); //button number
+ bhit = bluemod.getc(); //1=hit, 0=release
+ if (bluemod.getc()==char(~('!' + 'B' + bnum + bhit))) { //checksum OK?
+ //myled = bnum - '0'; //current button number will appear on LEDs
+ switch (bnum) {
+ case '1': //number button 1
+ if (bhit=='1') {
+ // nop
+ } else {
+ // nop
+ }
+ break;
+ case '2': //number button 2
+ if (bhit=='1') {
+ // nop
+ } else {
+ // nop
+ }
+ break;
+ case '3': //number button 3
+ if (bhit=='1') {
+ // nop
+ } else {
+ // nop
+ }
+ break;
+ case '4': //number button 4
+ if (bhit=='1') {
+ // nop
+ } else {
+ // nop
+ }
+ break;
+ case '5': //button 5 up arrow
+ if (bhit=='1' && sensorValue <= 0.6) {
+ //forward
+ //hit button
+ forward_led=1; //Set forward LED on MBED
+ fwdA = 1; //Assert MotorA forward
+ revA = 0; //Deassert MotorA reverse
+ speedA = 1; //Set SpeedA to full
+ fwdB = 1; //Assert MotorB forward
+ revB = 0; //Deassert MotorB reverse
+ speedB = 1; //Set SpeedB to full
+ } else {
+ //Let go of button
+ forward_led = 0; //Turn off forward LED
+ speedA = 0; // Turn off MotorA
+ speedB = 0; // Turn off MotorB
+ }
+ break;
+ case '6': //button 6 down arrow
+ if (bhit=='1') {
+ //reverse
+ //hit button
+ reverse_led=1; //Turn reverse LED on
+ fwdA = 0; //Deassert MotorA forward
+ revA = 1; //Assert MotorA reverse
+ speedA = 1; //MotorA full speed
+ fwdB = 0; //Deassert MotorB forward
+ revB = 1; //Assert MotorB reverse
+ speedB = 1; //MotorB full speed
+
+
+ } else {
+ //let go of button
+ reverse_led=0; //Turn reverse LED off
+ speedA = 0; //Turn off MotorA
+ speedB = 0; //Turn off MotorB
+ }
+ break;
+ case '7': //button 7 left arrow
+ if (bhit=='1') {
+ //ccw
+ //hit button
+ left_led=1; //Turn left LED on
+ fwdA = 1; //Assert MotorA forward
+ revA = 0; //Deassert MotorA reverse
+ speedA = 1; //MotorA full speed
+ fwdB = 0; //Deassert MotorB forward
+ revB = 1; //Assert MotorB reverse
+ speedB = 1; //MotorB full speed
+
+
+ } else {
+ //let go of button
+ left_led=0; //Turn left LED off
+ speedA = 0; // Turn MotorA off
+ speedB = 0; // Turn MotorB off
+ }
+ break;
+ case '8': //button 8 right arrow
+ if (bhit=='1') {
+ //cw
+ //hit button
+ right_led=1; //Turn right LED on
+ fwdA = 0; //Deassert MotorA forward
+ revA = 1; //Assert MotorA reverse
+ speedA = 1; //MotorA full speed
+ fwdB = 1; //Assert MotorB forward
+ revB = 0; //Deassert MotorB reverse
+ speedB = 1; //MotorB full speed
+
+ } else {
+ //let go of button
+ right_led=0; //Turn right LED off
+ speedA = 0; //Turn MotorA off
+ speedB = 0; //Turn MotorB off
+ }
+ break;
+ default:
+ break;
+ }
+ }
+ break;
+ }
+
+ }
+
+ }
+ }
+}
\ No newline at end of file