Revision 0:8681037b9a18, committed 2013-06-09

Comitter:

gabdo

Date:

Sun Jun 09 22:13:59 2013 +0000

Child:

1:9b90e7de6e09

Child:

2:bcb7c45ed3c2

Commit message:

quadCommand!

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Jun 09 22:13:59 2013 +0000
@@ -0,0 +1,8 @@
+#include "quadCommand.h"
+
+int main() 
+{
+    quadCommand quad;   // Create quadCommand object.
+    quad.run();         // Start quadCommand running.
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Sun Jun 09 22:13:59 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/b3110cd2dd17
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/quadCommand/com/com.cpp	Sun Jun 09 22:13:59 2013 +0000
@@ -0,0 +1,142 @@
+/****************************** com.cpp **********************************/
+/* Version: 1.0                                                          */
+/* Last Updated: June 1, 2013                                            */
+/*                                                                       */
+/* The com class implements reliable data transfer between two nodes     */
+/*using a checksum and a sequence number for guaranteed message delivery */
+/*over an xbee modem connected to the passed in tx and rx pins. Messages */
+/*are received and placed in the rxBuffer to be read when convenient.    */
+/*Messages are encoded by sending a byte with the value of the command   */
+/*then and int of the command.                                           */
+/*                                                                       */
+/* Commands:    0 -> Ack, does not get placed in rxQueue.                */
+/*              1 -> Throttle                                            */
+/*              2 -> Pitch                                               */
+/*              3 -> Roll                                                */
+/*              4 -> Yaw                                                 */
+/*************************************************************************/
+
+#include "mbed.h"
+#include "com.h"
+
+/*********************** com( PinName, PinName ) *************************/
+/*                                                                       */
+/*************************************************************************/
+
+com::com( PinName tx, PinName rx ) : xbee( tx, rx)
+{
+    bLength = 0;                            // How many bytes are in the buffer.
+    xbee.attach( this, &com::callback );    // Set callback as the interrupt handler. 
+    xbee.baud(BAUDRATE);                    // Setup the serial baud rate.
+    rxBuffer = new queue();                     // Point to the rxQueue.
+
+}
+
+/************************* bool isData()  ********************************/
+/*                                                                       */
+/*************************************************************************/
+
+bool com::isData()
+{
+    if( rxBuffer->isEmpty() )
+        return false;
+        
+    return true;
+}
+
+/************************ void write( char ) *****************************/
+/* Write a packet out the xbee com port.                                 */
+/*                                                                       */
+/* Data format byte[]                                                    */
+/*                byte[0] = command.                                     */
+/*                byte[1] = upper 8 bits of value.                       */
+/*                byte[2] = lower 8 bits of value.                       */
+/*                byte[3] = Checksum byte[0] + byte[2].                  */
+/*                byte[4] = Sequence Number.                             */
+/*                byte[5] = 255 End of message.                          */
+/*************************************************************************/
+
+void com::write( short command, short value  )
+{
+    xbee.putc( (char)command );     // Command
+    xbee.putc( 0 );                 // First 8 bits in array. 
+    xbee.putc( (char)value );       // Second 8 bits in array.
+    xbee.putc( command + value );   // Checksum array[0] + array[1].
+    xbee.putc( 0 );                 // Sequence number.
+    xbee.putc( 255 );               // End of message.
+}
+
+/*************************** char read()  ********************************/
+/*                                                                       */
+/*************************************************************************/
+
+short * com::read()
+{
+    // Are there commands in the readBuffer queue?    
+    if( !rxBuffer->isEmpty())
+        return rxBuffer->pop();
+        
+    return NULL;
+}
+
+/************************ void callback() ********************************/
+/*                                                                       */
+/*************************************************************************/
+
+void com::callback()
+{   
+    while( xbee.readable() )
+    {
+        char data = xbee.getc(); 
+        if( bLength++ < BUFFERSIZE )
+            buffer[bLength] = data;      
+     
+        if( data == 255 )
+            packetBuilder();
+    }
+}
+
+/********************** void packetBuilder() *****************************/
+/* Creates a packet from the buffered data and places it in the rxBuffer */
+/*queue to be read whenever convenient. Max value of +/- 8063.           */
+/*************************************************************************/
+
+void com::packetBuilder()
+{
+    char * commandData = new char[bLength];
+    commandData[4] = buffer[--bLength];     // Sequence Number.
+    commandData[3] = buffer[--bLength];     // CheckSum value.
+    commandData[2] = buffer[--bLength];     // Second 7 bits.
+    commandData[1] = buffer[--bLength];     // Fisrt 7 bits.
+    commandData[0] = buffer[--bLength];     // Command.
+            
+    if( commandData[0] + commandData[2] == commandData[3] ) // Validate checksum.
+    {
+        short * array = new short[2];
+        
+        array[0] = (short)commandData[0];
+        
+        short value = (short)(commandData[1] * 128 + commandData[2]);
+        
+        if( value > 8062 )
+            value = (short)value + 57344;
+        
+        array[1] = value;
+        
+        rxBuffer->add( array );   // Add to read buffer.
+        write( 0, (short)commandData[4]); // Ack the packet with sequence nuber.
+    } 
+    delete[] commandData;
+    bLength = 0;    // Reset the buffer length.                                           
+}
+
+
+
+
+
+
+
+
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/quadCommand/com/com.h	Sun Jun 09 22:13:59 2013 +0000
@@ -0,0 +1,47 @@
+/******************************* com.h ***********************************/
+/* Version: 1.0                                                          */
+/* Last Updated: June 1, 2013                                            */
+/*                                                                       */
+/* The com class implements reliable data transfer between two nodes     */
+/*using a checksum and a sequence number for guaranteed message delivery */
+/*over an xbee modem connected to the passed in tx and rx pins. Messages */
+/*are received and placed in the rxBuffer to be read when convenient.    */
+/*Messages are encoded by sending a byte with the value of the command   */
+/*then and int of the command.                                           */
+/*                                                                       */
+/* Commands:    0 -> Ack, does not get placed in rxQueue.                */
+/*              1 -> Throttle                                            */
+/*              2 -> Pitch                                               */
+/*              3 -> Roll                                                */
+/*              4 -> Yaw                                                 */
+/*************************************************************************/
+
+#ifndef COM_H
+#define COM_H
+
+#include "mbed.h"
+#include "queue.h"
+
+const int BAUDRATE      = 38400;
+const int BUFFERSIZE    = 10;
+
+class com
+{  
+    public:
+        com( PinName, PinName  );   // Setup the com serial port. (tx, rx)
+        bool isData();              // Is there data to be read?
+        void write( short, short ); // Write to the port.
+        short * read();             // Read from the queue.
+        
+    private:   
+        void callback();            // Handle the interrupts.
+        void packetBuilder();       // Called by callback to place commandes into the queue.
+        
+        char buffer[BUFFERSIZE];    // Buffer for holding serial data.
+        int bLength;                // Location in the buffer to place next data.
+        Serial xbee;                // tx - DIN, rx - DOUT
+        
+        queue *rxBuffer;            // queue of commands ready to be read.
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/quadCommand/com/queue/queue.cpp	Sun Jun 09 22:13:59 2013 +0000
@@ -0,0 +1,148 @@
+/*************************** queue.cpp ***************************************/
+/*                                                                           */
+/*  Authers: Oanh Tran, Ling Lei, Sihao Xie, Greg Abdo.                      */
+/*  Date:    February 23, 2013                                               */
+/*  Version: 1.0                                                             */
+/*                                                                           */
+/* The queue is used to stack StructureItem in order with a FILO             */
+/*arrangement.                                                               */
+/*****************************************************************************/
+
+#include "queue.h"
+
+/***************************** constructor ***********************************/
+/* Description:                                                              */
+/*****************************************************************************/
+
+queue::queue()
+{
+    front = NULL;   // Set front to NULL at the start.
+}
+
+/******************************* distructor **********************************/
+/* Description:                                                              */
+/*****************************************************************************/
+
+queue::~queue()
+{
+    clear();        // Clear the entire queue.
+}
+
+/*****************************************************************************/
+/* Description:                                                              */
+/* Accepts:                                                                  */
+/* Returns:                                                                  */
+/*****************************************************************************/
+
+bool queue::isEmpty()
+{
+    // Is the queue empty?
+    if( front == NULL ) // Check the front pointer.
+        return true;    // Queue is empty, return true.
+    
+    return false;       // There is atleast one item, not empty.
+}
+
+/*****************************************************************************/
+/* Description:                                                              */
+/* Accepts:                                                                  */
+/* Returns:                                                                  */
+/*****************************************************************************/
+
+void queue::clear()
+{
+    // Is the list already empty?
+    if( isEmpty() )     // Check for an empty list.
+        return;         // List is empty, don't need to do anything.
+
+    queueNode * current = front;    // Create node to help step through list.
+    queueNode * placeHold = front;  // Create node to keep place of delete.
+
+    // As long as were not at the end, keep stepping to the next node.
+    while( current != NULL)
+    {
+        placeHold = current->next;  // Hold where we have to go.
+        delete current;             // Delete the node.
+        current = placeHold;        // Set current to the next node.
+    }
+
+    front = NULL;   // Reset the front to NULL;
+    length = 0;
+}
+
+/*****************************************************************************/
+/* Description:                                                              */
+/* Accepts:                                                                  */
+/* Returns:                                                                  */
+/*****************************************************************************/
+
+void queue::add( short * item )
+{
+    // Were we passed an invalid object somehow?
+    if( item == NULL )  // Check for NULL
+        return;         // If so, return.
+
+    if( queueLength() > MAXQUEUELENGTH )
+        clear();
+        
+    queueNode * newNode = new queueNode( item );    // Create the new node.
+
+    if( isEmpty() )
+        front = newNode;        // Set front to the new node.
+        
+    else    
+    {
+        queueNode *temp = front;
+        while( temp->next != NULL )
+            temp = temp->next;
+            
+        temp->next = newNode;
+    }   
+    length++;
+}
+
+/*****************************************************************************/
+/* Description:                                                              */
+/* Accepts:                                                                  */
+/* Returns:                                                                  */
+/*****************************************************************************/
+
+short * queue::pop()
+{
+    // Is the list already empty?
+    if( isEmpty() )     // Check for an empty list.
+        return NULL;    // List is empty, don't need to do anything.
+
+    short* dataHold = front->data;  // Keep track of what were returning. 
+    queueNode * oldNode = front;    // Save the old node to be deleted
+    front = front->next;            // Set front to next object.
+                                    
+    delete oldNode;                 // Delete the front node.                     
+    length--;                       // Remove one from the length.
+    return dataHold;                // return the stuctureItem.
+}
+
+/*****************************************************************************/
+/* Description:                                                              */
+/* Accepts:                                                                  */
+/* Returns:                                                                  */
+/*****************************************************************************/
+
+short * queue::peek()
+{
+    if( front != NULL )
+        return front->data;
+
+    return NULL;
+}
+
+/*****************************************************************************/
+/* Description:                                                              */
+/* Accepts:                                                                  */
+/* Returns:                                                                  */
+/*****************************************************************************/
+
+short queue::queueLength()
+{
+    return length;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/quadCommand/com/queue/queue.h	Sun Jun 09 22:13:59 2013 +0000
@@ -0,0 +1,53 @@
+/**************************** queue.h ****************************************/
+/*                                                                           */
+/*  Authers: Greg Abdo.                                                      */
+/*  Date:    February 23, 2013                                               */
+/*  Version: 1.0                                                             */
+/*                                                                           */
+/* The queue is used to stack StructureItem in order with a FILO arrangement.*/
+/*****************************************************************************/
+
+#ifndef QUEUE_H
+#define QUEUE_H
+
+#include "mbed.h"
+
+using namespace std;
+
+const int MAXQUEUELENGTH = 5;
+
+class queue
+{
+public: 
+    queue();                        // Queue constructor
+    ~queue();                       // Queue destructor
+
+    bool isEmpty();                 // Check for an empty queue.
+    void clear();                   // Clears the entire queue.
+    void add( short* );              // Push commandData into the queue.
+    short* peek();                   // Look at the last item in the queue.
+    short* pop();                    // Pop the top item off the queue.
+    short queueLength();              // Return how many objects are in the queue.
+
+private:
+    int length;
+    
+    struct queueNode                // Node object for the queue.
+    {
+        queueNode( short* array )
+        {
+            data = array;
+            next = NULL;
+        }
+
+        ~queueNode()
+        {}
+
+        short* data;                 // Pointer to the StructureItem object.
+        queueNode * next;           // Next node in the queue.
+    };  
+
+    queueNode * front;              // Root of the queue.
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/quadCommand/motor/motor.cpp	Sun Jun 09 22:13:59 2013 +0000
@@ -0,0 +1,74 @@
+/****************************** motor.cpp ********************************/
+/* Version: 1.0                                                          */
+/* Last Updated: June 1, 2013                                            */
+/*                                                                       */
+/* The motor class is used for motor control using a PWM ECS. When a     */
+/*a motor object is created you must pass the PWM pin as the only        */
+/*argument.                                                              */
+/*************************************************************************/
+
+#include "motor.h"
+
+/************************** motor( PinName ) *****************************/
+/* The motor constructor takes the pin to be used as for PWM and sets it */
+/*to default safe valuse.                                                */
+/*************************************************************************/
+
+motor::motor( PinName pin ) : _pwm(pin)
+{
+    _pwm.period( 0.020 );       // Set the period to 20ms.
+    setPulseMin( 0.001150 );    // Set default min pulse.
+    setPulseMax( 0.002 );       // Set default max pulse.
+    setSpeed( 0 );              // Set motor to stopped.
+}
+
+/************************** setSpeed( int ) ******************************/
+/* Set speed takes an int value between 0 and 100 and sets the speed of  */
+/*the motor based on passed in percent value of speed.                   */
+/*************************************************************************/
+
+void motor::setSpeed( int value )
+{
+    // Is the value to small?
+    if( value < 0 )             // Yup, just set to 0.
+        currentSpeed = 0;
+    
+    // Is the value to large?
+    else if( value > 100 )      // Yup, just set to 100.
+        currentSpeed = 100;
+    
+    // Value must be in the correct range.    
+    else    
+        currentSpeed = value;   // Set the new value.
+
+    // Calculate the value based on pulseMin, pulseMax and currentSpeed.
+    pulse = ((pulseMax - pulseMin ) / 100 * currentSpeed ) + pulseMin;
+    _pwm.pulsewidth( pulse );   // Write the pulse to the pin.
+}
+
+/************************** setPulseMin( float ) *************************/
+/*                                                                       */
+/*************************************************************************/
+
+void motor::setPulseMin( float value )
+{
+    pulseMin = value;
+}
+
+/************************** setPulseMax( float ) *************************/
+/*                                                                       */
+/*************************************************************************/
+
+void motor::setPulseMax( float value )
+{
+    pulseMax = value;
+}
+
+/*************************** float getPulse( ) ***************************/
+/* Get the current pulse value.                                          */
+/*************************************************************************/
+
+float motor::getPulse( void )
+{
+    return pulse;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/quadCommand/motor/motor.h	Sun Jun 09 22:13:59 2013 +0000
@@ -0,0 +1,41 @@
+/******************************* motor.h *********************************/
+/* Version: 1.0                                                          */
+/* Last Updated: June 1, 2013                                            */
+/*                                                                       */
+/* The motor class is used for motor control using a PWM ECS. When a     */
+/*a motor object is created you must pass the PWM pin as the only        */
+/*argument.                                                              */
+/*                                                                       */
+/*  Wiring diagrams                                                      */
+/*                 _______                     _______                   */
+/*       CW   ----|       |     CCW   ---\/ --|       |                  */
+/*            ----| Motor |           ---/\---| Motor |                  */
+/*            ----|_______|           --/  \--|_______|                  */
+/*            Straight through       Switch wire 1 and 3                 */
+/*                                                                       */
+/*************************************************************************/
+
+  
+#ifndef MOTOR_H
+#define MOTOR_H
+
+#include "mbed.h"
+
+class motor
+{   
+    public:
+        motor( PinName );           // motor object constructor.
+        void setSpeed( int );       // Set the speed for the motor 0-100
+        void setPulseMin( float );  // Set smallest pulse.
+        void setPulseMax( float );  // Set largest pulse.
+        float getPulse( void );
+    
+    private:   
+        PwmOut _pwm;                // Pin used for PWM.
+        int currentSpeed;           // Speed of the motor.
+        float pulse;                // Current pulse of the motor.
+        float pulseMin;             // Shortest value for the pulse
+        float pulseMax;             // Largest value for the pulse..
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/quadCommand/quadCommand.cpp	Sun Jun 09 22:13:59 2013 +0000
@@ -0,0 +1,98 @@
+/************************ quadCommand.cpp ********************************/
+/*                                                                       */
+/*************************************************************************/
+
+#include "quadCommand.h"
+
+quadCommand::quadCommand()
+{
+        myCom = new com( TXPIN, RXPIN );        // Setup com object.       
+        world = new sensors();                  // Setup the sensors.
+        
+        myMotors[0] = new motor( MOTOR1 );      // Connect motor 0 to PTD4 pin.
+        myMotors[1] = new motor( MOTOR2 );      // Connect motor 1 to PTA12 pin.
+        myMotors[2] = new motor( MOTOR3 );      // Connect motor 2 to PTA4 pin.
+        myMotors[3] = new motor( MOTOR4 );      // Connect motor 3 to PTA5 pin.
+        
+        rxThrottle    = 0;
+        rxPitch       = 0;
+        rxRoll        = 0;
+        rxYaw         = 0;
+}
+
+void quadCommand::run() 
+{   
+    while(1)
+    {
+        if( myCom->isData() )
+            rxInput();
+            
+        //myCom->write( 3, world->getAbsoluteY());       
+        updatePosition();
+         
+        if( globalUpdate )
+            txPosition();
+    }
+}
+
+void quadCommand::rxInput()
+{
+    short * command = myCom->read();
+            
+    if( command[0] == 1 )       // Throttle command.
+        rxThrottle = command[1];
+                
+    if( command[0] == 2 )       // Pitch command.
+        rxPitch = command[1];
+                
+    if( command[0] == 3 )       // Roll command.
+        rxRoll = command[1];            
+            
+     if( command[0] == 4 )      // Yaw command.
+        rxYaw = command[1];           
+            
+     myMotors[ 0 ]->setSpeed( rxThrottle + rxPitch - rxRoll );    // Motor 1
+     myMotors[ 1 ]->setSpeed( rxThrottle + rxPitch + rxRoll );    // Motor 2
+     myMotors[ 2 ]->setSpeed( rxThrottle - rxPitch + rxRoll );    // Motor 3
+     myMotors[ 3 ]->setSpeed( rxThrottle - rxPitch - rxRoll );    // Motor 4
+                         
+     delete[] command;
+}
+
+void quadCommand::updatePosition()
+{
+    float temp;
+    
+    temp = world->getAbsoluteX();
+    if( temp != currentRoll )
+    {
+        currentRoll = temp;
+        updatedRoll = true;
+        globalUpdate = true;
+    }
+    
+    temp = world->getAbsoluteY();
+    if( temp != currentPitch )
+    {
+        currentPitch = temp;
+        updatedPitch = true;
+        globalUpdate = true;
+    }
+}
+
+void quadCommand::txPosition()
+{
+    if( updatedRoll )
+    {
+        myCom->write( 3, currentRoll);
+        updatedRoll = false;
+    }
+        
+    if( updatedPitch )
+    {
+        myCom->write( 2, currentPitch);
+        updatedPitch = false;
+    }
+    
+    globalUpdate = false;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/quadCommand/quadCommand.h	Sun Jun 09 22:13:59 2013 +0000
@@ -0,0 +1,52 @@
+/************************* quadCommand.h *********************************/
+/*                                                                       */
+/*************************************************************************/
+
+#ifndef QUAD_COMMAND_H
+#define QUAD_COMMAND_H
+
+#include "mbed.h"
+#include "motor.h"
+#include "com.h"
+#include "sensors.h"
+
+const PinName MOTOR1    =   PTD4;   // Pin used for motor 1.
+const PinName MOTOR2    =   PTA12;  // Pin used for motor 2.
+const PinName MOTOR3    =   PTA4;   // Pin used for motor 3.
+const PinName MOTOR4    =   PTA5;   // Pin used for motor 4.
+
+const PinName TXPIN     =   PTD3;   // Pin used for xbee TX.
+const PinName RXPIN     =   PTD2;   // Pin used for xbee RX.
+
+class quadCommand 
+{
+    public:
+        quadCommand();          // Constructor.
+        void run();             // Loop.
+        void rxInput();         // Deal with new input from xbee.
+        void updatePosition();  
+        void txPosition();
+    
+    private:
+        motor *myMotors[4];     // Array of motor objects.
+        com *myCom;             // The com object.
+        sensors *world;         // Sensors used to observe the world.
+    
+        short rxThrottle;       // Throttle position: 0 -> 100.
+        short rxPitch;          // Pitch:   -180 -> 180.
+        short rxRoll;           // Roll:    -180 -> 180.
+        short rxYaw;            // Yaw:     -180 -> 180.
+        
+        bool globalUpdate;
+        
+        short currentPitch;     //
+        bool updatedPitch;
+        
+        short currentRoll;      //
+        bool updatedRoll;
+        
+        short currentAlt;       //
+        bool updatedAlt;
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/quadCommand/sensorS/MMA8451Q.lib	Sun Jun 09 22:13:59 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/JoKer/code/MMA8451Q/#2d14600116fc

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/quadCommand/sensorS/sensors.cpp	Sun Jun 09 22:13:59 2013 +0000
@@ -0,0 +1,20 @@
+/************************ quadCommand.cpp ********************************/
+/*                                                                       */
+/*************************************************************************/
+
+#include "sensors.h"
+
+sensors::sensors()
+{
+     acc = new MMA8451Q( ACCSDA, ACCSCL, MMA8451_I2C_ADDRESS);
+}
+
+short sensors::getAbsoluteX()
+{
+    return (short)(acc->getAccX() * 100);
+} 
+
+short sensors::getAbsoluteY()
+{
+    return (short)(acc->getAccY() * 100);
+} 
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/quadCommand/sensorS/sensors.h	Sun Jun 09 22:13:59 2013 +0000
@@ -0,0 +1,24 @@
+/************************* quadCommand.h *********************************/
+/*                                                                       */
+/*************************************************************************/
+
+#include "MMA8451Q.h"
+
+#ifndef SENSORS_H
+#define SENSORS_H
+
+#define MMA8451_I2C_ADDRESS (0x1d<<1)
+const PinName ACCSDA    =   PTE25; 
+const PinName ACCSCL    =   PTE24;
+
+class sensors
+{
+    public:
+        sensors();
+        short getAbsoluteX();
+        short getAbsoluteY();
+    
+    private:
+        MMA8451Q *acc;
+};
+#endif
\ No newline at end of file