Luis Bernal
Xbeat beta code as published by Andre Moehl ( http://mbed.org/users/hoppel/ ) With a slightly modified serial port definition
Diff: main.cpp
- Revision:
- 0:badcd0d61c7b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Feb 04 08:31:00 2011 +0000
@@ -0,0 +1,488 @@
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ * @file main.c
+ * @author andre moehl
+ * @date 01/2011
+ *
+ *
+ * Implements the main functionalty and als programs for the XBeat.
+ */
+
+/*--- Includes ------------------------------*/
+#include "mbed.h"
+#include "SRF05.h"
+#include "drive.h"
+#include "button.h"
+
+
+/*--- Defines -------------------------------*/
+#define DEBUG 1
+
+#define TTLSERIAL 1 //use the ttl(p13,p14) serial port or the native USB serial port if 0
+#define BAUDRATE 19200
+
+#define OFF 0
+#define ON 1
+
+// Battery measure
+#define UBAT_R_FACTOR 3.1363636 //resistor divider
+//#define UBAT_LOW 66 //6,6V
+#define UBAT_LOW 72 //7,2V - more adequate value to protect lipos
+
+
+// Servo Zero Position Offset
+#define OFFSET_L -35
+#define OFFSET_R 45
+
+
+
+/*--- Global Variables ----------------------*/
+/* Hardware Definitions */
+DigitalOut led1(LED1); ///< Leds on mbed board
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+DigitalOut Speaker(p12); ///< Loudspeaker
+AnalogIn ubat(p15); ///< LiPo Voltage Measurement
+DigitalOut T6(p29); ///< FET for Bottom IR-LEDs
+DigitalOut T7(p30); ///< FET for Side IR-LEDs
+AnalogIn IRL(p16); ///< IR-Transistor Bottom Left
+AnalogIn IRM(p17); ///< IR-Transistor Bottom Middle
+AnalogIn IRR(p18); ///< IR-Transistor Bottom Right
+AnalogIn Left(p19); ///< IR-Transistor Left Side
+AnalogIn Right(p20); ///< IR-Transistor Right Side
+Button SW1(p10); ///< Button Outer Side
+Button SW2(p9); ///< Button Inner Side
+
+#ifdef TTLSERIAL
+Serial pc (p13,p14); ///< Serial Interface via TTL Port
+#else
+Serial pc(USBTX, USBRX); ///< Serial Intervace via USB Port (tx, rx)
+#endif
+
+/** Timer and Tickers */
+Ticker LiPoTicker; //ticker for checking LiPoVoltage
+Ticker tick_1ms; //Ticker for checking Button
+
+/*--- Flags ---------------------------------*/
+
+int SW1_state=0; // Current State of SW1
+int SW1_old=0; // Previous State of SW1
+int SW2_state=0; // Current State of SW2
+int SW2_old=0; // Previous State of SW2
+int start=0; // indicate to start a program
+int stop=1; // indicate to stop a program
+int programm_counter=1; // Counts the number of SW2 samples to select a program
+uint8_t firstUnderVoltage; // set to 1 when the battery measure is first time under the limit
+
+
+/**--- predeclarared Functions --------------*/
+void setLed(int led);
+float getLiPoVoltage();
+
+/**--- Functions ----------------------------*/
+
+////// PROGRAMM LINE FOLLOW ///////////////////////////////////////////
+
+#define LFDUTYCYCLE 0.001
+/**
+* @brief Programm LineFollow follows lines on paper. Uses the IR-Transistors to detect.
+* @parameter mode Fast or Slow
+*/
+void LineFollow() {
+ Drive XBeat(p21,p22); //Left, Right Servo Motor
+ int i;
+ int ML=0, MR=0; //Motor set-points
+ int MaxSpeed=0; ///< Maximum Speed in Percent
+ unsigned short left=0, right=0, mid=0; // current measured values
+ unsigned short soll_left=0, soll_right=0, soll_mid=0; // must values
+ signed int error_r=0,error_l=0,error_m=0; // setpoint errors
+ signed int prev_error_r=0, prev_error_l=0, prev_error_m=0; // previous setpoint errors
+ signed int dmeas_r=0, dmeas_l=0, dmeas_m=0; // derive error
+ signed int pid_r=0, pid_l=0, pid_m=0; // calced controller values
+
+
+ MaxSpeed = 100; //Set Maximum Line Follow Speed
+
+ pc.printf("Program \"Line Follow\"\n\r\n\r");
+ XBeat.setoffset(OFFSET_L,OFFSET_R);
+ T6 = ON; //switch IR-Leds on
+
+ //read values for calc the set point values
+ soll_left = IRL.read_u16()/100;
+ soll_right = IRM.read_u16()/100;
+ soll_mid = IRM.read_u16()/100;
+ for (i=30;i>0;i--) {
+ soll_left = (soll_left + IRL.read_u16()/100)/2;
+ soll_right = (soll_right + IRR.read_u16()/100)/2;
+ soll_mid = (soll_mid + IRM.read_u16()/100)/2;
+ }
+ //debug
+ pc.printf("Sollwert Links: %d\n\r", soll_left);
+ pc.printf("Sollwert Rechts: %d\n\r", soll_right);
+ wait(2);
+ //peep
+ Speaker = 1;
+ wait (0.1);
+ Speaker = 0;
+ //acceleration
+ int ms = 500/MaxSpeed;
+ for (i=0; i<MaxSpeed; i+=2) {
+ XBeat.move(i,i);
+ wait_ms(ms);
+ }
+
+ //drive until pressing stop button
+ while (!stop) {
+ //set SetPoint
+ MR = MaxSpeed;
+ ML = MaxSpeed;
+
+ //get sensor data
+ left = IRL.read_u16()/100;
+ right = IRR.read_u16()/100;
+ mid = IRM.read_u16()/100;
+
+ //calc controller (PD)
+ error_l = soll_left - left;
+ dmeas_l = (error_l - prev_error_l)/2;
+ prev_error_l = error_l;
+ error_r = soll_right - right;
+ dmeas_r = (error_r - prev_error_r)/2;
+ prev_error_r = error_r;
+ error_m = soll_mid - mid;
+ dmeas_m = (error_m - prev_error_m)/2;
+ prev_error_m = error_m;
+
+ pid_l = (2*error_l + dmeas_l)/20;
+ pid_r = (2*error_r + dmeas_r)/20;
+ pid_m = (2*error_m + dmeas_m)/10;
+
+ //Limit controller output
+ if (pid_l >= MaxSpeed) pid_l = MaxSpeed;
+ if (pid_l <= -MaxSpeed) pid_l = -MaxSpeed;
+ if (pid_r >= MaxSpeed) pid_r = MaxSpeed;
+ if (pid_r <= -MaxSpeed) pid_r = -MaxSpeed;
+ //debug
+ pc.printf("pidL %d, pidR%d\n\r", pid_l, pid_r);
+
+ if (pid_l > 2) ML = ML + pid_m;
+ if (pid_r > 2) MR = MR + pid_m;
+
+ ML = ML - pid_l;// - pid_r/2;
+ MR = MR - pid_r;// - pid_l/2;
+ if (ML > MaxSpeed) {
+ ML = MaxSpeed;
+ MR = MR + pid_l;
+ }
+ if (MR > MaxSpeed) {
+ MR = MaxSpeed;
+ ML = ML + pid_r;
+ }
+ //Bottom Limit Motor values
+ if (ML < -10) ML = -10;
+ if (MR < -10) MR = -10;
+
+ pc.printf("ML %d, MR%d\n\r", ML, MR);
+
+ //set Motor values
+ XBeat.move(ML, MR); //set motor speed
+ wait(LFDUTYCYCLE);
+ }
+ //Finished
+ T6 = OFF;
+ start = 0;
+ stop = 1;
+ pc.printf("Program \"Line Follow\" stopped\n\r");
+}
+
+////// PROGRAMM REMOTE CONTROL //////////////////////////////////////////
+
+/**
+* @brief Control the XBeat via remote
+*/
+void RemoteControl() {
+ Drive XBeat(p22,p21); //Left, Right Servo Motor
+ pc.printf("Program \"Remote Control\" started\n\r");
+ XBeat.setoffset(OFFSET_L,OFFSET_R);
+ pc.printf("Currently not implemented.\n\r");
+ start = 0;
+ stop = 1;
+ pc.printf("Program \"Remote Control\" stopped\n\r");
+}
+
+
+////// PROGRAMM MAZE SOLVER /////////////////////////////////////////////
+
+#define MAZE_SPEED 60
+#define MAZEDUTYCYCLE 0.01
+/**
+ * @brief drives the XBeat thru a maze
+ * made for 27C3 Maze Logo
+ */
+void MazeSolver() {
+ Drive XBeat(p22,p21); //Left, Right Servo Motor
+ pc.printf("Program \"Maze Solver\" started\n\r");
+ XBeat.setoffset(OFFSET_L,OFFSET_R);
+ pc.printf("The Implementation is currently not finished. I working on it and release this later.\n\r");
+ start = 0;
+ stop = 1;
+ pc.printf("Program \"Maze Solver\" stopped\n\r");
+}
+
+///// PROGRAMM DRIVE AND TURN /////////////////////////////////////////////
+
+void driver() {
+ SRF05 srf(p6, p5); // Front Sensor
+ Drive XBeat(p21,p22); // Left, Right Servo Motor
+ signed int m1=0,m2=0; // Motor Set values
+ float front=0, front_old=0; // measured value from front sensor
+ int turn=0; // flag for indicate a turn
+ int ticks=0; // loop counter for turn
+
+ pc.printf("\n\rJust Moving\n\r\n\r");
+ XBeat.setoffset(OFFSET_L,OFFSET_R);
+
+ while (!stop) {
+ if (!turn) {
+ if ((front < front_old) && (front > 8.0)) { //close to a front wall, slow stopping
+ if (m1 < 0) m1 = m1-1;
+ if (m2 < 0) m2 = m2-1;
+ pc.printf("1\n\r");
+ } else {
+ if (m1 < 100) m1++;
+ if (m2 < 100) m2++;
+ }
+ front_old = front;
+ }
+
+ if (turn) {
+ ticks++;
+ if (ticks <= 300) {
+ if (m1 < 50) m1++;
+ if (m2 > -50) m2--;
+ } else {
+ if (m1 >= 0) m1--;
+ if (m2 <= 0) m2++;
+ if (m1 == 0 || m2 == 0) {
+ turn=0;
+ ticks=0;
+ }
+ }
+ }
+
+ if (front < 8.0 && !turn) {
+ m1=0;
+ m2=0;
+ turn=1;
+ ticks=0;
+ }
+ XBeat.move(m1,m2);
+ }
+ start = 0;
+ stop = 1;
+ pc.printf("Program \"Driving\" stopped\n\r");
+}
+
+
+/////// Sensor Data ///////////////////////////////////////////////////////
+
+/**
+* @brief Writing Sensor Data to UART, for debugging purpose
+*/
+void SensorData() {
+ SRF05 srf(p6, p5);
+ T6 = ON;
+ T7 = ON;
+ pc.printf("Sensor Data\n\r\n\r");
+ while (!stop) {
+ pc.printf("IR-Left %d, IR-Right %d, SRF05 %f\n\r", Left.read_u16()/100, Right.read_u16()/100, srf.read());
+ pc.printf("IRL %d, IRM %d, IRR %d\n\r", IRL.read_u16()/100, IRM.read_u16()/100, IRR.read_u16()/100);
+ pc.printf("LiPo Voltage %f\n\r\n\r", getLiPoVoltage() );
+ wait (1);
+ }
+ T7 = OFF;
+ T6 = OFF;
+ start = 0;
+ stop = 1;
+}
+
+
+/////// RECALLED FUNCTIONS ////////////////////////////////////
+/**
+ * @brief function called ervery 1ms to read the button states
+ */
+void checkButtons () {
+ //int c=0;
+ if (SW1) {
+ SW1_state = 1;
+ } else SW1_state = 0;
+ if (SW1_old == 1 && SW1_state == 0) { //falling edge
+ /** Add Code to do when Button 1 is pressed */
+ start = !start;
+ stop = !start;
+ }
+ SW1_old = SW1_state;
+
+ if (SW2) SW2_state = 1;
+ else SW2_state = 0;
+ if (SW2_old == 1 && SW2_state == 0) { //falling edge
+ /** Add Code to do when Button 2 is pressed */
+ if (stop) {
+ if (programm_counter < 5) programm_counter++;
+ else programm_counter = 1;
+
+ switch (programm_counter) {
+ case 1:
+ pc.printf("Programm %d: Line Follow\n\r", programm_counter );
+ break;
+ case 2:
+ pc.printf("Programm %d: Maze Solver\n\r", programm_counter );
+ break;
+ case 3:
+ pc.printf("Programm %d: Driving\n\r", programm_counter );
+ break;
+ case 4:
+ pc.printf("Programm %d: Remote Control\n\r", programm_counter );
+ break;
+ case 5:
+ pc.printf("Programm %d: Sensor Data\n\r", programm_counter );
+ break;
+ }
+ if (programm_counter == 5) {
+ led1=1;
+ led4=1;
+ } else setLed(programm_counter);
+ }
+ }
+ SW2_old = SW2_state;
+}
+
+
+
+/** @brief calculates the battery voltage
+* @return real voltage als float */
+float getLiPoVoltage() {
+ return ((ubat*UBAT_R_FACTOR*3.3)+0.79);
+}
+
+/**@brief function called every 1s to check LiPo Voltage */
+void LiPo_Warning() {
+ int c = 0;
+ //LiPo Undervoltage Emergency Stop
+ if ((getLiPoVoltage()*10) < UBAT_LOW) {
+
+ start=0;
+ stop=1;
+ if (firstUnderVoltage) { //need to return 1st time to stop the motors
+ firstUnderVoltage = 0;
+ return;
+ } else {
+ while (1) { //stay here
+ Speaker = 1;
+ led4 = 1;
+ wait(0.1);
+ led4 = 0;
+ Speaker = 0;
+ wait(1);
+ if (c++ == 5) {
+ pc.printf("LiPo Voltage: %fV\n\r",getLiPoVoltage());
+ c=0;
+ }
+ }
+ }
+ }
+}
+
+//////// HELPER ///////////////////////////////////////////////////////////
+/** @ brief set the LEDs on the mbed dev board
+ * @parameter led - LED Number (1-4)
+ */
+void setLed(int led) {
+ led1=OFF;
+ led2=OFF;
+ led3=OFF;
+ led4=OFF;
+
+ switch (led) {
+ case 1:
+ led1 = ON;
+ break;
+ case 2:
+ led2 = ON;
+ break;
+ case 3:
+ led3 = ON;
+ break;
+ case 4:
+ led4 = ON;
+ break;
+ }
+}
+
+
+
+
+/** @brief Initializations */
+void init() {
+ pc.baud(BAUDRATE); //set uart baudrate
+ tick_1ms.attach_us(&checkButtons, 1000); //ervery 1ms
+ firstUnderVoltage = 1; // set flag
+ LiPoTicker.attach(&LiPo_Warning,1); //check LiPo voltage every second
+}
+
+///////// MAIN ///////////////////////////////////////////////////////////
+
+/**
+* @brief main function
+*/
+int main() {
+ init();
+ setLed(programm_counter);
+ pc.printf("---- XBeat ----\n\r\n\r");
+ pc.printf("Initial Program 1: Line Follow\n\r");
+ while (1) {
+ wait(0.1);
+ if (start) {
+ switch (programm_counter) {
+ case 1:
+ LineFollow();
+ setLed(programm_counter);
+ break;
+ case 2:
+ MazeSolver();
+ setLed(programm_counter);
+ break;
+ case 3:
+ driver();
+ setLed(programm_counter);
+ break;
+ case 4:
+ RemoteControl();
+ setLed(programm_counter);
+ break;
+ case 5:
+ SensorData();
+ setLed(programm_counter++);
+ break;
+ default:
+ break;
+ }
+ }
+ }
+}
+