(x) > (max) ? (max) : (x))\r
\r
#define MMA8451_I2C_ADDRESS (0x1d<<1) // acc sensor address\r
-#define TIME_ACCURACY 0.01\r
+#define TIME_ACCURACY 0.0005\r
\r
-#define ACC_SNS_DEFAULT 0.1 // default collecting interval in seconds\r
-#define MAG_SNS_DEFAULT 1.0\r
-#define LGT_SNS_DEFAULT 1.0\r
-#define TCH_SNS_DEFAULT 1.0\r
-#define A2D_SNS_DEFAULT 1.0\r
+#define ACC_SNS_DEFAULT 0.01 // default collecting interval in seconds\r
+#define MAG_SNS_DEFAULT 0.1\r
+#define LGT_SNS_DEFAULT 0.1\r
+#define TCH_SNS_DEFAULT 0.1\r
+#define A2D_SNS_DEFAULT 0.1\r
+#define LED_OUT_DEFAULT 0.5\r
\r
#define MIN_RATE 1E-4 // 1 kHz\r
#define MAX_RATE 10.0 // 0.1 Hz\r
\r
#define MAX_FRAME_SIZE 256\r
\r
+#define SNS_NUM 5\r
+#define TYP_NUM 8\r
+#define CMD_NUM 3\r
+\r
typedef enum {\r
- ACC_SNS,\r
- MAG_SNS,\r
- LGT_SNS,\r
- TCH_SNS,\r
- A2D_SNS,\r
+ SNS_ACC,\r
+ SNS_MAG,\r
+ SNS_LGT,\r
+ SNS_TCH,\r
+ SNS_A2D,\r
} sns_t;\r
\r
typedef enum {\r
- INT,\r
- LONG,\r
- FLOAT,\r
- DOUBLE,\r
+ TYP_S8, TYP_S16, TYP_S32,\r
+ TYP_U8, TYP_U16, TYP_U32,\r
+ TYP_F32, TYP_F64,\r
} type_t;\r
\r
typedef enum {\r
- START,\r
- STOP,\r
- SET_INT,\r
-} oper_t;\r
+ CMD_STOP,\r
+ CMD_START,\r
+ CMD_RATE,\r
+} cmd_t;\r
\r
// Data Frame Information\r
+#pragma pack(1)\r
typedef struct {\r
uint8_t header;\r
struct {\r
- uint8_t sns : 5;\r
- uint8_t type : 3;\r
+ uint8_t type : 4;\r
+ uint8_t sns : 4;\r
} bits;\r
uint8_t count;\r
uint8_t data[];\r
} state_t;\r
+#pragma pack()\r
\r
// Command Frame Information\r
+#pragma pack(1)\r
typedef struct {\r
uint8_t header;\r
struct {\r
- uint8_t sns : 5;\r
- uint8_t oper : 3;\r
+ uint8_t cmd : 4;\r
+ uint8_t sns : 4;\r
} bits;\r
float interval;\r
-} command_t;\r
+} control_t;\r
+#pragma pack()\r
\r
// Define Devices & Pins\r
-MMA8451Q accSensor(PTE25, PTE24, MMA8451_I2C_ADDRESS);\r
-MAG3110 magSensor(PTE25, PTE24);\r
-TSISensor tchSensor;\r
-AnalogIn lgtSensor(PTE22);\r
-AnalogIn a2dSensor(A0);\r
+MMA8451Q accSensor(PTE25, PTE24, MMA8451_I2C_ADDRESS);\r
+MAG3110 magSensor(PTE25, PTE24);\r
+TSISensor tchSensor;\r
+AnalogIn lgtSensor(PTE22);\r
+\r
+AnalogIn a2dSensor0(A0);\r
+AnalogIn a2dSensor1(A1);\r
+AnalogIn a2dSensor2(A2);\r
+AnalogIn a2dSensor3(A3);\r
+AnalogIn a2dSensor4(A4);\r
+AnalogIn a2dSensor5(A5);\r
+\r
+AnalogOut a2dOutput(PTE30);\r
\r
-Serial serial(USBTX, USBRX);\r
-Ticker clock1;\r
+DigitalOut led1(LED1);\r
+DigitalOut led2(LED2);\r
+\r
+Serial serial(USBTX, USBRX);\r
+Ticker clock1;\r
\r
// Global Variables\r
// Initial interval: in seconds\r
float tchTmr = TCH_SNS_DEFAULT;\r
float lgtTmr = LGT_SNS_DEFAULT;\r
float a2dTmr = A2D_SNS_DEFAULT;\r
+float ledTmr = LED_OUT_DEFAULT;\r
\r
-bool accEnable = true;\r
+bool accEnable = false;\r
bool magEnable = false;\r
bool lgtEnable = false;\r
bool tchEnable = false;\r
-bool a2dEnable = false;\r
+bool a2dEnable = true;\r
+bool ledEnable = true;\r
\r
-bool useStr = true;\r
+bool useStr = false;\r
bool useHex = false;\r
-bool useBin = false;\r
+bool useBin = true;\r
\r
uint8_t txFrame[MAX_FRAME_SIZE];\r
uint8_t rxFrame[MAX_FRAME_SIZE];\r
void clock1_interrupt(void);\r
void serialRx_interrupt(void);\r
\r
+void runCommand(control_t *cmd);\r
+\r
void sendAccInfo(void);\r
void sendMagInfo(void);\r
void sendLgtInfo(void);\r
int main(void) {\r
// Initialization\r
// Interruption Declarations\r
- clock1.attach(&clock1_interrupt, TIME_ACCURACY); // maximun accuracy be 0.1s\r
+ //clock1.attach(&clock1_interrupt, TIME_ACCURACY); // maximun accuracy be 0.1s\r
serial.attach(&serialRx_interrupt, Serial::RxIrq); // receive interrupt for serialS\r
- serial.baud(115200);\r
+ //serial.baud(115200);\r
+ serial.baud(230400);\r
magSensor.begin();\r
\r
sdma_setup(UART0, 0);\r
\r
printStr("\r\n============= Start of the program ============\r\n");\r
\r
+ int ticks = 0, tocks = 0;\r
while(1){\r
- wait(3);\r
+ ticks = (us_ticker_read()*1E-6) / TIME_ACCURACY;\r
+ if (tocks < ticks) {\r
+ clock1_interrupt();\r
+ tocks++;\r
+ }\r
}\r
}\r
\r
**********************/\r
\r
void serialRx_interrupt(void) { // Byte version\r
- clock1.detach(); // close the interrupt temporarily\r
- int i = 0;\r
- uint8_t ch = serial.getc();\r
-\r
- while (ch!=HEADER){\r
- if (serial.readable()){\r
- ch = serial.getc();\r
- }\r
- else{\r
- printStr("[ERROR] broken data!\r\n");\r
- clock1.attach(&clock1_interrupt, TIME_ACCURACY);\r
- return;\r
+ static int state = 0;\r
+ static int index = 0;\r
+ static int length = 0;\r
+\r
+ control_t *cmd = (control_t*)rxFrame;\r
+\r
+ while (serial.readable()) {\r
+ rxFrame[index++] = serial.getc();\r
+ switch (state) {\r
+ case 0: // Header\r
+ if (cmd->header == HEADER)\r
+ state = 1;\r
+ else\r
+ index = 0;\r
+ break;\r
+ case 1: // Bits\r
+ if (cmd->bits.sns >= SNS_NUM ||\r
+ cmd->bits.cmd >= CMD_NUM) {\r
+ state = index = 0;\r
+ } else if (cmd->bits.cmd == CMD_RATE) {\r
+ length = 4;\r
+ state = 2;\r
+ } else {\r
+ state = 3;\r
+ }\r
+ break;\r
+ case 2: // Data\r
+ if (--length == 0)\r
+ state = 3;\r
+ break;\r
+ case 3: // Tail\r
+ if (rxFrame[index-1] == TAIL)\r
+ runCommand(cmd);\r
+ state = 0;\r
+ index = 0;\r
+ break;\r
}\r
}\r
-\r
- //TODO: ticker for time out\r
- while (serial.readable()){\r
- rxFrame[i] = ch;\r
- if (ch=='\n')\r
- break;\r
- ch = serial.getc();\r
- i++;\r
- }\r
- rxFrame[++i] = '\0';\r
-\r
- // Cast to command and\r
- command_t *cmd = (command_t *)rxFrame;\r
-\r
- // Validate interval\r
- sns_t snsType = (sns_t )cmd->bits.sns;\r
- oper_t operType = (oper_t)cmd->bits.oper;\r
- float interval = CLAMP(cmd->interval, MIN_RATE, MAX_RATE);\r
-\r
- // Save value to global data\r
- switch(snsType){\r
- case ACC_SNS:\r
- switch(operType){\r
- case START: accEnable = true; break;\r
- case STOP: accEnable = false; break;\r
- case SET_INT: accTmr = interval; break;\r
- }\r
- break;\r
-\r
- case MAG_SNS:\r
- switch(operType){\r
- case START: magEnable = true; break;\r
- case STOP: magEnable = false; break;\r
- case SET_INT: magTmr = interval; break;\r
- }\r
- break;\r
-\r
- case LGT_SNS:\r
- switch(operType){\r
- case START: lgtEnable = true; break;\r
- case STOP: lgtEnable = false; break;\r
- case SET_INT: lgtTmr = interval; break;\r
- }\r
- break;\r
-\r
- case TCH_SNS:\r
- switch(operType){\r
- case START: tchEnable = true; break;\r
- case STOP: tchEnable = false; break;\r
- case SET_INT: tchTmr = interval; break;\r
- }\r
- break;\r
-\r
- case A2D_SNS:\r
- switch(operType){\r
- case START: a2dEnable = true; break;\r
- case STOP: a2dEnable = false; break;\r
- case SET_INT: a2dTmr = interval; break;\r
- }\r
- break;\r
- }\r
-\r
- clock1.attach(&clock1_interrupt, TIME_ACCURACY);\r
}\r
\r
void clock1_interrupt(void){\r
static int lgtCnt;\r
static int tchCnt;\r
static int a2dCnt;\r
+ static int ledCnt;\r
+ static int sinCnt;\r
\r
- accCnt++;\r
- magCnt++;\r
- lgtCnt++;\r
- tchCnt++;\r
- a2dCnt++;\r
+ static float sinAmp = 0.5; // 0..1\r
+ static float sinFreq = 0.25; // Hz\r
+\r
+ // Write A2D output sine wave\r
+ a2dOutput.write(sinAmp*sin(sinCnt * TIME_ACCURACY * (2*PI) * sinFreq)*0.5 + 0.5);\r
\r
- // TODO: send data through Serial\r
- if (accEnable && (accCnt<0 || accCnt>=accTmr/TIME_ACCURACY)){\r
+ // Send data through Serial\r
+ if (accEnable && accCnt >= (int)(accTmr/TIME_ACCURACY+0.5)){\r
sendAccInfo();\r
accCnt = 0;\r
}\r
- if (magEnable && (magCnt<0 || magCnt>=magTmr/TIME_ACCURACY)){\r
+ if (magEnable && magCnt >= (int)(magTmr/TIME_ACCURACY+0.5)){\r
sendMagInfo();\r
magCnt = 0;\r
}\r
- if (lgtEnable && (lgtCnt<0 || lgtCnt>=lgtTmr/TIME_ACCURACY)){\r
+ if (lgtEnable && lgtCnt >= (int)(lgtTmr/TIME_ACCURACY+0.5)){\r
sendLgtInfo();\r
lgtCnt = 0;\r
}\r
- if (tchEnable && (tchCnt<0 || tchCnt>=tchTmr/TIME_ACCURACY)){\r
+ if (tchEnable && tchCnt >= (int)(tchTmr/TIME_ACCURACY+0.5)){\r
sendTchInfo();\r
tchCnt = 0;\r
}\r
- if (a2dEnable && (a2dCnt<0 || a2dCnt>=a2dTmr/TIME_ACCURACY)){\r
+ if (a2dEnable && a2dCnt >= (int)(a2dTmr/TIME_ACCURACY+0.5)){\r
sendA2dInfo();\r
a2dCnt = 0;\r
}\r
\r
+ // Toggel LED for debugging\r
+ if (ledEnable && ledCnt>=ledTmr/TIME_ACCURACY){\r
+ led1 = !led1;\r
+ ledCnt = 0;\r
+ }\r
+\r
+ accCnt++;\r
+ magCnt++;\r
+ lgtCnt++;\r
+ tchCnt++;\r
+ a2dCnt++;\r
+ ledCnt++;\r
+ sinCnt++;\r
+\r
sdma_flush();\r
}\r
\r
+/*******************\r
+ * Command handler *\r
+ *******************/\r
+\r
+void runCommand(control_t *cmd)\r
+{\r
+ // Validate interval\r
+ sns_t snsType = (sns_t)cmd->bits.sns;\r
+ cmd_t cmdType = (cmd_t)cmd->bits.cmd;\r
+ float interval = CLAMP(cmd->interval, MIN_RATE, MAX_RATE);\r
+\r
+ // Save value to global data\r
+ switch(snsType){\r
+ case SNS_ACC:\r
+ switch(cmdType){\r
+ case CMD_STOP: accEnable = false; break;\r
+ case CMD_START: accEnable = true; break;\r
+ case CMD_RATE: accTmr = interval; break;\r
+ }\r
+ break;\r
+\r
+ case SNS_MAG:\r
+ switch(cmdType){\r
+ case CMD_STOP: magEnable = false; break;\r
+ case CMD_START: magEnable = true; break;\r
+ case CMD_RATE: magTmr = interval; break;\r
+ }\r
+ break;\r
+\r
+ case SNS_LGT:\r
+ switch(cmdType){\r
+ case CMD_STOP: lgtEnable = false; break;\r
+ case CMD_START: lgtEnable = true; break;\r
+ case CMD_RATE: lgtTmr = interval; break;\r
+ }\r
+ break;\r
+\r
+ case SNS_TCH:\r
+ switch(cmdType){\r
+ case CMD_STOP: tchEnable = false; break;\r
+ case CMD_START: tchEnable = true; break;\r
+ case CMD_RATE: tchTmr = interval; break;\r
+ }\r
+ break;\r
+\r
+ case SNS_A2D:\r
+ switch(cmdType){\r
+ case CMD_STOP: a2dEnable = false; break;\r
+ case CMD_START: a2dEnable = true; break;\r
+ case CMD_RATE: a2dTmr = interval; break;\r
+ }\r
+ break;\r
+ }\r
+}\r
+\r
/*******************\r
* Sensors reading *\r
*******************/\r
accData[0] = accSensor.getAccX();\r
accData[1] = accSensor.getAccY();\r
accData[2] = accSensor.getAccZ();\r
- int len = packToFrame(txFrame, ACC_SNS, FLOAT, 3, accData);\r
+ int len = packToFrame(txFrame, SNS_ACC, TYP_F32, 3, accData);\r
\r
printStr("[ACC] accX=%-2.4f accY=%-2.4f accZ=%-2.4f\r\n",\r
accData[0], accData[1], accData[2]);\r
}\r
\r
void sendMagInfo(void){\r
- int magData[3];\r
- magSensor.getValues(&magData[0], &magData[1], &magData[2]);\r
- int len = packToFrame(txFrame, MAG_SNS, INT, 3, magData);\r
-\r
- printStr("[MAG] magX=%d magY=%d magZ=%d\r\n",\r
- magData[0], magData[1], magData[2]);\r
+ // magSensor uses the wrong types,\r
+ // so we have to convert it\r
+ int tmp[3];\r
+ magSensor.getValues(&tmp[0], &tmp[1], &tmp[2]);\r
+ uint16_t magData[3];\r
+ magData[0] = tmp[0];\r
+ magData[1] = tmp[1];\r
+ magData[2] = tmp[2];\r
+ int len = packToFrame(txFrame, SNS_MAG, TYP_S16, 3, magData);\r
+\r
+ printStr("[MAG] magX=%hd magY=%hd magZ=%hd %d\r\n",\r
+ magData[0], magData[1], magData[2], sizeof(int));\r
printHex(txFrame, len);\r
printBin(txFrame, len);\r
}\r
\r
void sendLgtInfo(void){\r
float lgtData = lgtSensor.read();\r
- int len = packToFrame(txFrame, LGT_SNS, FLOAT, 1, &lgtData);\r
+ int len = packToFrame(txFrame, SNS_LGT, TYP_F32, 1, &lgtData);\r
\r
printStr("[LGT] intensity=%f\r\n",\r
lgtSensor.read());\r
float tchData[2];\r
tchData[0] = tchSensor.readPercentage();\r
tchData[1] = tchSensor.readDistance();\r
- int len = packToFrame(txFrame, TCH_SNS, FLOAT, 2, tchData);\r
+ int len = packToFrame(txFrame, SNS_TCH, TYP_F32, 2, tchData);\r
\r
printStr("[TCH] force=%0.4f distance=%2.2f\r\n",\r
tchData[0], tchData[1]);\r
\r
void sendA2dInfo(void){\r
float a2dData[6];\r
- a2dData[0] = 0;\r
- a2dData[1] = 0;\r
- a2dData[2] = 0;\r
- a2dData[3] = 0;\r
- a2dData[4] = 0;\r
- a2dData[5] = 0;\r
- int len = packToFrame(txFrame, A2D_SNS, FLOAT, 2, a2dData);\r
+ a2dData[0] = a2dSensor0.read();\r
+ a2dData[1] = a2dSensor1.read();\r
+ a2dData[2] = a2dSensor2.read();\r
+ a2dData[3] = a2dSensor3.read();\r
+ a2dData[4] = a2dSensor4.read();\r
+ a2dData[5] = a2dSensor5.read();\r
+ int len = packToFrame(txFrame, SNS_A2D, TYP_F32, 6, a2dData);\r
\r
printStr("[A2D] data=%2.2f %2.2f %2.2f %2.2f %2.2f %2.2f\r\n",\r
a2dData[0], a2dData[1], a2dData[2],\r
* Helper functions *\r
********************/\r
\r
-int calDataSize(uint8_t dataType){\r
+int calDataSize(type_t dataType){\r
switch(dataType){\r
- case INT: return 2;\r
- case LONG: return 4;\r
- case FLOAT: return 4;\r
- case DOUBLE: return 8;\r
+ case TYP_S8: return 1;\r
+ case TYP_S16: return 2;\r
+ case TYP_S32: return 4;\r
+ case TYP_U8: return 1;\r
+ case TYP_U16: return 2;\r
+ case TYP_U32: return 4;\r
+ case TYP_F32: return 4;\r
+ case TYP_F64: return 8;\r
}\r
return 4;\r
}\r
*******************/\r
\r
int packToFrame(uint8_t *frame, sns_t snsType, type_t dataType, int dataNum, void *data){\r
- int size = dataNum + calDataSize(dataType);\r
+ //const char pattern[] = "\x80\x81\x82\x83"\r
+ // "\x84\x85\x86\x87"\r
+ // "\x88\x89\x8A\x8B"\r
+ // "\x8C\x8D\x8E\x8F";\r
+\r
+ int size = dataNum * calDataSize(dataType);\r
state_t *state = (state_t*)frame;\r
uint8_t *tail = &state->data[size];\r
\r
projects / ~andy / csm213a-hw / blobdiff