Diff: main.cpp

Revision:

0:84d5aa80fd77

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Apr 27 04:31:07 2014 +0000
@@ -0,0 +1,203 @@
+#include "mbed.h"
+
+//Radio Calibration Signals - Need to implement calibration routine...
+#define CHAN2_MID 1470  //Elevator Midpoint
+#define CHAN1_MID 1470  //Rudder Midpoint
+#define DEADZONE 105
+#define MAXMIN_OFFSET 400
+
+//Drive Modes
+typedef enum {FORWARD, REVERSE, STOP, CENTERLEFT, CENTERRIGHT} DRIVEMODE;
+DRIVEMODE mode;
+float xpwm, ypwm;
+
+//H-bridge
+PwmOut rightMotorPWM(p22);  //Channel A
+PwmOut leftMotorPWM(p21);   //Channel B
+DigitalOut leftMotor1(p23);
+DigitalOut leftMotor2(p24);
+DigitalOut rightMotor1(p25);
+DigitalOut rightMotor2(p26);
+
+//Radio 
+Serial pc(USBTX, USBRX);
+DigitalIn rudder(p11);
+DigitalIn elevator(p12);
+
+//Xbee
+Serial xbee(p13, p14);
+DigitalOut xbeeReset(p15);
+
+void setTurnLeft(float speed) {
+    //Set speed
+    rightMotorPWM = speed;
+    leftMotorPWM = speed;
+    
+    //Set motor function
+    leftMotor1 = 0;
+    leftMotor2 = 1;
+    rightMotor1 = 1;
+    rightMotor2 = 0;
+}
+
+void setTurnRight(float speed) {
+    //Set speed
+    rightMotorPWM = speed;
+    leftMotorPWM = speed;
+    
+    //Set motor function
+    leftMotor1 = 1;
+    leftMotor2 = 0;
+    rightMotor1 = 0;
+    rightMotor2 = 1;
+}
+
+//Stop with braking
+void stop() {
+    rightMotorPWM = 0;
+    leftMotorPWM = 0;
+    leftMotor1 = 0;
+    leftMotor2 = 1;
+    rightMotor1 = 1;
+    rightMotor2 = 0;
+}
+
+
+int getRadioX() {
+    Timer timer;
+    timer.reset();
+    int dur;
+    while(!rudder);
+    timer.start();
+    while(rudder);
+    dur = timer.read_us();
+    return dur;
+}
+
+int getRadioY() {
+    Timer timer;
+    timer.reset();
+    int dur;
+    while(!elevator);
+    timer.start();
+    while(elevator);
+    dur = timer.read_us();
+    return dur;
+}
+
+//Primary function for setting motors for radio driving
+void radioDrive() {
+    int radioY = getRadioY();
+    int radioX = getRadioX();
+    
+    //Identify drive mode from radio values
+    if (radioY < CHAN2_MID - DEADZONE)
+        mode = FORWARD;
+    if (radioY > CHAN2_MID + DEADZONE)
+        mode = REVERSE;
+    if (radioY < CHAN2_MID + DEADZONE && radioY > CHAN2_MID - DEADZONE &&
+        radioX < CHAN1_MID - DEADZONE)
+        mode = CENTERLEFT;
+    if (radioY < CHAN2_MID + DEADZONE && radioY > CHAN2_MID - DEADZONE &&
+        radioX > CHAN1_MID + DEADZONE)
+        mode = CENTERRIGHT;        
+    if (radioY < CHAN2_MID + DEADZONE && radioY > CHAN2_MID - DEADZONE &&
+        radioX < CHAN1_MID + DEADZONE && radioX > CHAN1_MID - DEADZONE)
+        mode = STOP;
+        
+    //Normalize values for PWM magnitude
+    xpwm = abs((float)radioX - CHAN1_MID) / MAXMIN_OFFSET;
+    ypwm = abs((float)radioY - CHAN2_MID) / MAXMIN_OFFSET;
+        
+    //Set Motors Accordingly
+    switch(mode) {
+        case FORWARD:
+            //Handle variable turns
+            if (radioX > CHAN1_MID + DEADZONE) {
+                rightMotorPWM = ypwm - 0.8*xpwm;
+                leftMotorPWM = ypwm;
+            } else if (radioX < CHAN1_MID - DEADZONE) {
+                rightMotorPWM = ypwm;
+                leftMotorPWM = ypwm - 0.8*xpwm;
+            } else {
+                rightMotorPWM = ypwm;
+                leftMotorPWM = ypwm; 
+            }
+            leftMotor1 =  0;
+            leftMotor2 = 1;
+            rightMotor1 = 0;
+            rightMotor2 = 1;
+            break;
+        
+        case REVERSE:
+            //Handle variable turns
+            if (radioX > CHAN1_MID + DEADZONE) {
+                rightMotorPWM = ypwm - 0.8*xpwm;
+                leftMotorPWM = ypwm;
+            } else if (radioX < CHAN1_MID - DEADZONE) {
+                rightMotorPWM = ypwm;
+                leftMotorPWM = ypwm - 0.8*xpwm;
+            } else {
+                rightMotorPWM = ypwm;
+                leftMotorPWM = ypwm; 
+            }
+            leftMotor1 =  1;
+            leftMotor2 = 0;
+            rightMotor1 = 1;
+            rightMotor2 = 0;
+            break;
+            
+        case CENTERRIGHT:
+            rightMotorPWM = xpwm;
+            leftMotorPWM = xpwm;
+            leftMotor1 = 1;
+            leftMotor2 = 0;
+            rightMotor1 = 0;
+            rightMotor2 = 1;
+            break;
+
+        case CENTERLEFT:
+            rightMotorPWM = xpwm;
+            leftMotorPWM = xpwm;
+            leftMotor1 = 0;
+            leftMotor2 = 1;
+            rightMotor1 = 1;
+            rightMotor2 = 0;
+            break;
+
+        case STOP:
+            rightMotorPWM = 0;
+            leftMotorPWM = 0;
+            leftMotor1 = 0;
+            leftMotor2 = 1;
+            rightMotor1 = 1;
+            rightMotor2 = 0;
+            break;
+    }
+}
+
+//Transmit position and heading
+void xbeeTelemetry() {
+    xbee.printf("Human Update \r\n");   
+    wait_ms(10);
+}
+
+bool isGameOver() {
+    return xbee.readable();
+}
+        
+
+
+int main() {
+    
+    //Init Xbee
+    xbeeReset = 0;
+    wait_ms(1);
+    xbeeReset = 1;
+    wait_ms(1);
+    
+    while(1) {
+        xbee.printf("HELLO!\r\n");
+        wait(0.1);
+    }
+}