chibios!

6 files changed, 581 insertions, 0 deletions

diff --git a/vex/digital.cpp b/vex/digital.cpp
new file mode 100644
index 0000000..f05edc3
--- /dev/null
+++ b/vex/digital.cpp
@@ -0,0 +1,37 @@
+#include "digital.hpp"
+
+#include <ch.h>
+#include <hal.h>
+
+// a lookup table for pads and ports
+static const int portPad[12][2] = {
+	{GPIOE_BASE,  9}, // digital pin 1
+	{GPIOE_BASE, 11}, // 2
+	{GPIOC_BASE,  6}, // ...
+	{GPIOC_BASE,  7},
+	{GPIOE_BASE, 13},
+	{GPIOE_BASE, 14},
+	{GPIOE_BASE,  8},
+	{GPIOE_BASE, 10},
+	{GPIOE_BASE, 12},
+	{GPIOE_BASE,  7},
+	{GPIOD_BASE,  0},
+	{GPIOD_BASE,  1},
+};
+
+namespace digital {
+	void setMode(int pin, int output) {
+		const auto& pp = portPad[pin - 1];
+		palSetPadMode(reinterpret_cast<GPIO_TypeDef*>(pp[0]), pp[1],
+		              output ? PAL_MODE_OUTPUT_PUSHPULL : PAL_MODE_INPUT_PULLUP);
+	}
+
+	void set(int pin, int val) {
+		const auto& pp = portPad[pin - 1];
+		if (val)
+			palSetPad(reinterpret_cast<GPIO_TypeDef*>(pp[0]), pp[1]);
+		else
+			palClearPad(reinterpret_cast<GPIO_TypeDef*>(pp[0]), pp[1]);
+	}
+}
+
diff --git a/vex/digital.hpp b/vex/digital.hpp
new file mode 100644
index 0000000..b1dd400
--- /dev/null
+++ b/vex/digital.hpp
@@ -0,0 +1,37 @@
+/**
+ * @file digital.hpp
+ * Provides functionality to control the digital ports.
+ */
+
+#ifndef DIGITAL_HPP_
+#define DIGITAL_HPP_
+
+namespace digital {
+	/**
+	 * Sets the pin to either input or output
+	 * @param pin pin to set mode of
+	 * @param output input if 0, output if not
+	 */
+	void setMode(int pin, int output);
+
+	/**
+	 * Sets the state of a pin.
+	 * DOESN'T CHECK FOR PIN MODE!
+	 * @param pin pin to set
+	 * @param val value to give it (true/false)
+	 */
+	void set(int pin, int val);
+
+	/**
+	 * Sets the state of an LED.
+	 * Basically set(), but flips the value as LEDs act that way.
+	 * @param pin pin to set
+	 * @param val value to give it
+	 * @see set
+	 */
+	inline void setLed(int pin, int val)
+	{ set(pin, !(val)); }
+}
+
+
+#endif // DIGITAL_HPP_
diff --git a/vex/lcd.cpp b/vex/lcd.cpp
new file mode 100644
index 0000000..00a1279
--- /dev/null
+++ b/vex/lcd.cpp
@@ -0,0 +1,93 @@
+#include "lcd.hpp"
+
+#include <ch.h>
+#include <hal.h>
+
+#include <library.hpp>
+
+// lcd config for chibios
+static SerialConfig lcdConfig = {
+	19200,
+	0,
+	USART_CR2_STOP1_BITS,
+	0
+};
+
+// buffer used to update lcd
+static char lcdLine[32];
+
+// signature for lcd buffer sent to LCD
+static const char lcdBufBase[5] = {
+	0xAA, 0x55, 0x1E, 0x12, 0,
+};
+
+// the lcd task, updates the LCD.
+static char lcdUpdateWA[512];
+static void lcdUpdate(void *unused)
+{
+	(void)unused;
+
+	static int line = 2; // 2/3 for backlight
+
+	// update function
+	auto lcdLineUpdate = [&](char *s) {
+		static char lcdBuf[22];
+		// buffer prep
+		strncpy(lcdBuf, lcdBufBase, 5);
+		for (int i = 5; i < 21; i++)
+			lcdBuf[i] = ' ';
+		strncpy(lcdBuf + 5, s, 16);
+		lcdBuf[4] = line;
+
+		// checksum creation
+		int cs = 0x100;
+		for (int i = 4; i < 21; i++)
+			cs -= lcdBuf[i];
+		lcdBuf[21] = cs;
+	
+		// write to lcd
+		sdWrite(&SD3, reinterpret_cast<unsigned char *>(lcdBuf), 22);
+		chThdSleepMilliseconds(15);
+
+		// read from lcd (buttons... (TODO))
+		if (sdGetWouldBlock(&SD3))
+			return;
+
+		for (int i = 0; i < 16; i++) {
+			auto c = sdGetTimeout(&SD3, TIME_IMMEDIATE);
+			if (c != Q_TIMEOUT)
+				lcdBuf[i] = c;
+			else
+				break;
+		}
+	};
+
+	// loop on line updates
+	while (1) {
+		lcdLineUpdate(lcdLine);
+		line++;
+		lcdLineUpdate(lcdLine + 16);
+		line--;
+	}
+}
+
+namespace lcd {
+	void init(void) {
+		sdStart(&SD3, &lcdConfig);
+		chThdCreateStatic(lcdUpdateWA, 512, NORMALPRIO - 1, lcdUpdate, nullptr);
+	}
+
+	void prints(int line, int x, const char *s) {
+		strncpy(lcdLine + (16 * line) + x, s, 16 - x);
+	}
+
+	void printn(int line, int x, int val) {
+		strncpy(lcdLine + (16 * line) + x, itoa(val), 16 - x);
+	}
+	
+	void flush(void) {
+		for (int i = 0; i < 32; i++)
+			lcdLine[i] = ' ';
+	}
+}
+
diff --git a/vex/lcd.hpp b/vex/lcd.hpp
new file mode 100644
index 0000000..1b58a69
--- /dev/null
+++ b/vex/lcd.hpp
@@ -0,0 +1,42 @@
+/**
+ * @file lcd.hpp
+ * Provides functionality to mess with the LCD (...on UART2).
+ */
+
+#ifndef LCD_HPP_
+#define LCD_HPP_
+
+namespace lcd {
+	/**
+	 * Initializes the LCD.
+	 */
+	void init(void);
+
+	/**
+	 * Prints a string to the given line and column.
+	 * NO ERROR CHECKING DONE!
+	 * @param line the line to write to
+	 * @param x the column to start on
+	 * @param s the string to write
+	 */
+	void prints(int line, int x, const char *s);
+
+	/**
+	 * Prints a decimal number to the given line and column.
+	 * NO ERROR CHECKING DONE!
+	 * @param line the line to write to
+	 * @param x the column to start on
+	 * @param val the number to write
+	 */
+	void printn(int line, int x, int val);
+
+	/**
+	 * Clears the LCD text buffer.
+	 * Called at the top of the loop lcd printing is done in.
+	 * TODO make something better
+	 */
+	void flush(void);
+}
+
+
+#endif // LCD_HPP_
diff --git a/vex/vexspi.cpp b/vex/vexspi.cpp
new file mode 100644
index 0000000..4d05b9e
--- /dev/null
+++ b/vex/vexspi.cpp
@@ -0,0 +1,201 @@
+/*-----------------------------------------------------------------------------*/

+/*                                                                             */

+/*                        Copyright (c) James Pearman                          */

+/*                                   2013                                      */

+/*                            All Rights Reserved                              */

+/*                                                                             */

+/*-----------------------------------------------------------------------------*/

+/*                                                                             */

+/*    Module:     vexspi.c                                                     */

+/*    Author:     James Pearman                                                */

+/*    Created:    7 May 2013                                                   */

+/*                                                                             */

+/*    Revisions:                                                               */

+/*                V1.00     4 July 2013 - Initial release                      */

+/*                                                                             */

+/*-----------------------------------------------------------------------------*/

+/*                                                                             */

+/*    This file is part of ConVEX.                                             */

+/*                                                                             */

+/*    The author is supplying this software for use with the VEX cortex        */

+/*    control system. ConVEX 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.                      */

+/*                                                                             */

+/*    ConVEX 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/>.    */

+/*                                                                             */

+/*    A special exception to the GPL can be applied should you wish to         */

+/*    distribute a combined work that includes ConVEX, without being obliged   */

+/*    to provide the source code for any proprietary components.               */

+/*    See the file exception.txt for full details of how and when the          */

+/*    exception can be applied.                                                */

+/*                                                                             */

+/*    The author can be contacted on the vex forums as jpearman                */

+/*    or electronic mail using jbpearman_at_mac_dot_com                        */

+/*    Mentor for team 8888 RoboLancers, Pasadena CA.                           */

+/*                                                                             */

+/*-----------------------------------------------------------------------------*/

+

+#include "ch.h"         // needs for all ChibiOS programs

+#include "hal.h"        // hardware abstraction layer header

+

+#include <vex/vexspi.h>

+#include <library.hpp>

+

+SpiData vexSpiData; // externed in main.cpp

+

+constexpr GPTDriver *spiGpt = &GPTD2;

+constexpr const char *spiTeamName = CONVEX_TEAM_NAME;

+

+static thread_t *spiThread = nullptr;

+

+static SPIConfig spicfg = {

+    nullptr,

+    /* HW dependent part.*/

+    VEX_SPI_CS_PORT, VEX_SPI_CS_PIN,

+    SPI_CR1_DFF | SPI_CR1_BR_2 | SPI_CR1_CPHA

+};

+

+static void _vspi_gpt_cb(GPTDriver *);

+static const GPTConfig vexSpiGpt = {

+	1000000,      /* 1MHz timer clock.*/

+	_vspi_gpt_cb, /* Timer callback.*/

+	0,            /* DIER = 0, version 2.6.1.and on */

+	0

+};

+

+static const unsigned char txInitData[32] = {

+	0x17, 0xC9, 0x02, 0x00, 0x00, 0x00,

+	0x7F, 0x7F, 0x7F, 0x7F, 0x7F, 0x7F, 0x7F, 0x7F,

+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,

+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,

+	0x01, 0x00

+};

+

+static void _vspi_gpt_cb(GPTDriver *gptp)

+{

+	(void)gptp;

+

+	chSysLockFromISR();

+	if (spiThread != nullptr) {

+		chSchReadyI(spiThread);

+		spiThread = nullptr;

+	}

+	chSysUnlockFromISR();

+}

+

+void vexSpiTickDelay(int ticks)

+{

+	chSysLock();

+	spiThread = chThdGetSelfX();

+	gptStartOneShotI(spiGpt, ticks);

+	chSchGoSleepS(CH_STATE_SUSPENDED);

+	chSysUnlock();

+}

+

+

+

+namespace spi {

+	void init(void) {

+		memncpy(&vexSpiData.txdata, txInitData, 32);

+		vexSpiData.online = 0;

+

+		spiStart(&SPID1, &spicfg);

+		gptStart(spiGpt, &vexSpiGpt);

+	}

+

+

+	void update(void) {

+		auto txbuf = reinterpret_cast<uint16_t *>(vexSpiData.txdata.data);

+		auto rxbuf = reinterpret_cast<uint16_t *>(vexSpiData.rxdata_t.data);

+

+		// configure team name

+		if (vexSpiData.txdata.pak.state == 0x03) {

+			// let spi know what we're sending

+			vexSpiData.txdata.pak.type = 0x55;

+			strncpy(reinterpret_cast<char *>(vexSpiData.txdata.data + 6), spiTeamName, 8);

+		}

+

+	    // Set handshake to indicate new spi message

+	    palSetPad(VEX_SPI_ENABLE_PORT, VEX_SPI_ENABLE_PIN);

+

+	    for (int i = 0; i < 16; i++) {

+			// exchange word (int)

+	        spiSelectI(&SPID1);

+	        rxbuf[i] = spi_lld_polled_exchange( &SPID1, txbuf[i] );

+	        spiUnselectI(&SPID1);

+

+			if (((i%4) == 3) && (i != 15)) {

+	            // long delay between each group of 4 words

+	            vexSpiTickDelay(73);

+

+	            // After 4 words negate handshake pin

+	            palClearPad(VEX_SPI_ENABLE_PORT, VEX_SPI_ENABLE_PIN);

+	        } else {

+	        	vexSpiTickDelay(8);

+			}

+	    }

+

+	    // increase id for next message

+	    vexSpiData.txdata.pak.id++;

+

+	    // check integrity of received data

+	    if( (vexSpiData.rxdata_t.data[0] == 0x17 ) && (vexSpiData.rxdata_t.data[1] == 0xC9 ))

+	        {

+	        // copy temporary data

+	        for(int i=0;i<32;i++)

+	            vexSpiData.rxdata.data[i] = vexSpiData.rxdata_t.data[i];

+

+	        // Set online status if valid data status set

+	        if( (vexSpiData.rxdata.pak.status & 0x0F) == 0x08 )

+    	        vexSpiData.online = 1;

+

+        	// If in configuration initialize state (0x02 or 0x03)

+	        if( (vexSpiData.txdata.pak.state & 0x0E) == 0x02 )

+	            {

+	            // check for configure request

+	            if( (vexSpiData.rxdata.pak.status & 0x0F) == 0x02 )

+	                vexSpiData.txdata.pak.state = 0x03;

+	            // check for configure and acknowledge

+	            if( (vexSpiData.rxdata.pak.status & 0x0F) == 0x03 )

+	                {

+	                vexSpiData.txdata.pak.state = 0x08;

+	                vexSpiData.txdata.pak.type  = 0;

+	                }

+	            // Either good or bad data force to normal transmission

+	            // status will either be 0x04 or 0x08

+	            if( (vexSpiData.rxdata.pak.status & 0x0C) != 0x00 )

+	                {

+	                vexSpiData.txdata.pak.state = 0x08;

+	                vexSpiData.txdata.pak.type  = 0;

+	                }

+    	        }

+	        }

+	    else

+	        vexSpiData.errors++;

+	}

+

+	void setMotors(uint8_t *data) {

+		memncpy(data, vexSpiData.txdata.pak.motor, 8);

+	}

+

+	const jsdata& getJoystick(int num) {

+		return (num == 2) ? vexSpiData.rxdata.pak.js_2 : vexSpiData.rxdata.pak.js_1;

+	}

+

+	int getBatteryMain(void) {

+		return vexSpiData.rxdata.pak.batt1 * 59; // in mV

+	}

+

+	int getBatteryBackup(void) {

+		return vexSpiData.rxdata.pak.batt2 * 59; 

+	}

+

+}

diff --git a/vex/vexspi.h b/vex/vexspi.h
new file mode 100644
index 0000000..aa7eeb9
--- /dev/null
+++ b/vex/vexspi.h
@@ -0,0 +1,171 @@
+/*-----------------------------------------------------------------------------*/

+/*                                                                             */

+/*                        Copyright (c) James Pearman                          */

+/*                                   2013                                      */

+/*                            All Rights Reserved                              */

+/*                                                                             */

+/*-----------------------------------------------------------------------------*/

+/*                                                                             */

+/*    Module:     vexspi.h                                                     */

+/*    Author:     James Pearman                                                */

+/*    Created:    7 May 2013                                                   */

+/*                                                                             */

+/*    Revisions:                                                               */

+/*                V1.00     4 July 2013 - Initial release                      */

+/*                                                                             */

+/*-----------------------------------------------------------------------------*/

+/*                                                                             */

+/*    This file is part of ConVEX.                                             */

+/*                                                                             */

+/*    The author is supplying this software for use with the VEX cortex        */

+/*    control system. ConVEX 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.                      */

+/*                                                                             */

+/*    ConVEX 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/>.    */

+/*                                                                             */

+/*    A special exception to the GPL can be applied should you wish to         */

+/*    distribute a combined work that includes ConVEX, without being obliged   */

+/*    to provide the source code for any proprietary components.               */

+/*    See the file exception.txt for full details of how and when the          */

+/*    exception can be applied.                                                */

+/*                                                                             */

+/*    The author can be contacted on the vex forums as jpearman                */

+/*    or electronic mail using jbpearman_at_mac_dot_com                        */

+/*    Mentor for team 8888 RoboLancers, Pasadena CA.                           */

+/*                                                                             */

+/*-----------------------------------------------------------------------------*/

+

+#ifndef __VEXSPI__

+#define __VEXSPI__

+

+/*-----------------------------------------------------------------------------*/

+/** @file    vexspi.h

+  * @brief   SPI communication to the master processor, macros and prototypes

+*//*---------------------------------------------------------------------------*/

+

+#define VEX_SPI_ENABLE_PORT GPIOA

+#define VEX_SPI_ENABLE_PIN  11

+#define VEX_SPI_CS_PORT     GPIOE

+#define VEX_SPI_CS_PIN      0

+

+/*-----------------------------------------------------------------------------*/

+/** @brief      default team name                                              */

+/*-----------------------------------------------------------------------------*/

+

+#define CONVEX_TEAM_NAME    "ZEPHYR  ";

+

+typedef struct _jsdata {

+	int Ch1 :8;

+	int Ch2 :8;

+	int Ch3 :8;

+	int Ch4 :8;

+

+	struct accel_t {

+		int y :8;

+		int x :8;

+		int z :8;

+	} __attribute__ ((packed)) accel;

+

+	char Btn5D	:1;

+	char Btn5U  :1;

+	char Btn6D  :1;

+	char Btn6U  :1;

+

+	char Reserved :4;

+

+	char Btn8D	:1;

+	char Btn8L  :1;

+	char Btn8U  :1;

+	char Btn8R  :1;

+	char Btn7D	:1;

+	char Btn7L  :1;

+	char Btn7U  :1;

+	char Btn7R  :1;

+

+	char Reserved2[2];

+

+} __attribute__ ((packed)) jsdata;

+

+/*-----------------------------------------------------------------------------*/

+/** @brief      SPI transmit data packet                                       */

+/*-----------------------------------------------------------------------------*/

+/** @details

+ *  Format of a SPI transmit packet - 16 words

+ */

+typedef union _spiTxPacket {

+    struct  _spiTxPak {

+        unsigned char h1;           ///< Header byte one

+        unsigned char h2;           ///< Header byte two

+        unsigned char state;        ///< Control state

+        unsigned char reserved[2];  ///< unknown data

+        unsigned char type;         ///< data type

+        unsigned char motor[8];     ///< motor pwm data

+        unsigned char pad[15];      ///< not used

+        unsigned char rev_lsb;      ///< revision of user code lsb

+        unsigned char rev_msb;      ///< revision of user code msb

+        unsigned char id;           ///< message id

+    } pak;                          ///< access spiTxPacket as named variables

+

+    unsigned char   data[32];       ///< access spiTxPacket as an array of char

+} spiTxPacket;

+

+/*-----------------------------------------------------------------------------*/

+/** @brief      SPI receive data packet                                        */

+/*-----------------------------------------------------------------------------*/

+/** @details

+ *  Format of a SPI receive packet - 16 words

+ */

+typedef union _spiRxPacket {

+    struct _spiRxPak {

+        unsigned char h1;           ///< Header byte one

+        unsigned char h2;           ///< Header byte two

+        unsigned char status;       ///< status

+        unsigned char ctl;          ///< status and control byte

+        unsigned char batt1;        ///< main battery level, 59mV per bit

+        unsigned char batt2;        ///< backup battery level, 59mV per bit

+        jsdata        js_1;         ///< data for main joystick

+        unsigned char pad;          ///< 1 byte padding

+        jsdata        js_2;         ///< data for partner joystick

+        unsigned char rev_lsb;      ///< revision of master code lsb

+        unsigned char rev_msb;      ///< revision of master code msb

+        unsigned char id;           ///< message id

+    } pak;                          ///< access spiRxPacke as named variables

+

+    unsigned char   data[32];       ///< access spiRxPacket as an array of char

+} spiRxPacket;

+

+/*-----------------------------------------------------------------------------*/

+/** @brief      SPI data                                                       */

+/*-----------------------------------------------------------------------------*/

+/** @details

+ *  All SPI related data collected in this structure

+ */

+struct SpiData {

+    spiTxPacket txdata;             ///< tx data packet

+    spiRxPacket rxdata;             ///< valid rx data packet

+    spiRxPacket rxdata_t;           ///< receive data packet, may have errors

+    uint16_t    online;             ///< online status

+    uint32_t    errors;             ///< number of packets received with error

+};

+

+namespace spi {

+	void init(void);

+	void update(void);

+

+	void setMotors(uint8_t *data);

+

+	const jsdata& getJoystick(int num);

+

+	int getBatteryMain(void);

+	int getBatteryBackup(void);

+}

+

+#endif  // __VEXSPI__