Prosper Van
added gy80 dcm
Dependencies: mbed DCM_AHRS_GY80 PID MMA8451Q
Fork of
quadCommand
by 
Greg Abdo
Revision 64:2b6399fe00f6, committed 2013-07-28
- Comitter:
- oprospero
- Date:
- Sun Jul 28 21:46:54 2013 +0000
- Parent:
- 63:f4cb482ac5d4
- Commit message:
- working state;
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/quadCommand/DCM_AHRS_GY80.lib Sun Jul 28 21:46:54 2013 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/oprospero/code/DCM_AHRS_GY80/#49cbf2acc4e6
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/quadCommand/PID.lib Sun Jul 28 21:46:54 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/gabdo/code/PID/#8172f254dff4
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/quadCommand/com/com.cpp Sun Jul 28 21:46:54 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()
+{
+ if( !rxBuffer->isEmpty())
+ return rxBuffer->pop();
+
+ return NULL;
+}
+
+/************************ void callback() ********************************/
+/* */
+/*************************************************************************/
+
+void com::callback()
+{
+ while( xbee.readable() )
+ {
+ char data = xbee.getc();
+// xbee.printf("data: %d\n\r", data);
+ 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;
+// xbee.printf("Cmd: %d,\tVal: %d\n\r,",array[0],array[1]);
+ 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 Jul 28 21:46:54 2013 +0000
@@ -0,0 +1,42 @@
+/******************************* 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. */
+/* Alternative Pins RX = PTA1, TX PTA2 */
+/*************************************************************************/
+
+#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 Jul 28 21:46:54 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 Jul 28 21:46:54 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 Jul 28 21:46:54 2013 +0000
@@ -0,0 +1,75 @@
+/****************************** 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 )
+{
+ pwmPin = new PwmOut( pin );
+ pwmPin->period( 0.020 ); // Set the period to 20ms.
+ setPulseMin( 0.001000 ); // 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 > 500 ) // Yup, just set to 100. Changed to 500 to increase
+ currentSpeed = 500; // motor control resolution
+
+ // 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) / 500 * currentSpeed) + pulseMin;
+ pwmPin->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 getSpeed( ) ***************************/
+/* */
+/*************************************************************************/
+
+int motor::getSpeed()
+{
+ return currentSpeed;
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/quadCommand/motor/motor.h Sun Jul 28 21:46:54 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.
+ int getSpeed();
+
+ private:
+ PwmOut *pwmPin; // 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/sensors/MMA8451Q.lib Sun Jul 28 21:46:54 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 Jul 28 21:46:54 2013 +0000
@@ -0,0 +1,33 @@
+/************************ quadCommand.cpp ********************************/
+/* */
+/*************************************************************************/
+#include "sensors.h"
+
+/***************************** sensors() *********************************/
+/* */
+/*************************************************************************/
+
+sensors::sensors()
+{
+ acc = new MMA8451Q( ACCSDA, ACCSCL, MMA8451_I2C_ADDRESS);
+}
+
+/************************** getAbsoluteX() *******************************/
+/* Returns a float from -1 to 1 for the value off level in the x */
+/*directoin. 0 = level. */
+/*************************************************************************/
+
+float sensors::getAbsoluteX()
+{
+ return acc->getAccX();
+}
+
+/************************** getAbsoluteY() *******************************/
+/* Returns a float from -1 to 1 for the value off level in the Y */
+/*directoin. 0 = level. */
+/*************************************************************************/
+
+float sensors::getAbsoluteY()
+{
+ return acc->getAccY();
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/quadCommand/sensors/sensors.h Sun Jul 28 21:46:54 2013 +0000
@@ -0,0 +1,21 @@
+/************************* quadCommand.h *********************************/
+/* */
+/*************************************************************************/
+#include "quadCommand.h"
+#include "MMA8451Q.h"
+
+#ifndef SENSORS_H
+#define SENSORS_H
+
+class sensors
+{
+ public:
+ sensors();
+ void sendData();
+ float getAbsoluteX(); // Get the value off level in the X directoin.
+ float getAbsoluteY(); // Get the value off level in the Y directoin.
+
+ private:
+ MMA8451Q *acc; // Pointer to the acceleromi
+};
+#endif
\ No newline at end of file