Instrumentation / Mbed 2 deprecated frdm_MasterVehicle

This is the source code used by master vehicle. Modules: CAMCTL SRVCTL MTRCTL still missing i2c readings from accelerometer and rf communication

Dependencies:   CAMCTL MTRCTL SRVCTL mbed FXOS8700CQ XBEE

main.cpp@4:0c97edd98157, 2014-10-18 (annotated)

Committer:
josepesado
Date:
Sat Oct 18 13:04:43 2014 +0000
Revision:
4:0c97edd98157
Parent:
3:7bdacc4cf273 
Integrated Tx of data through XBee services.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JalilChavez 1:a457798861a4 1 /**
JalilChavez 1:a457798861a4 2 * File: frdm_MasterVehicle/main.cpp
JalilChavez 1:a457798861a4 3 *
JalilChavez 1:a457798861a4 4 * Author1: Carla Amaya
JalilChavez 1:a457798861a4 5 * Author2: Gerardo Cordero
JalilChavez 1:a457798861a4 6 * Author3: José Pesado
JalilChavez 1:a457798861a4 7 * Author3: Jalil Chávez
JalilChavez 1:a457798861a4 8 *
JalilChavez 1:a457798861a4 9 * Date: October 2014
JalilChavez 1:a457798861a4 10 * Course: Instrumentation
JalilChavez 1:a457798861a4 11 *
JalilChavez 1:a457798861a4 12 * Summary of File:
JalilChavez 1:a457798861a4 13 *
JalilChavez 1:a457798861a4 14 * This file contains code which controlls a line follower.
JalilChavez 1:a457798861a4 15 * This device takes as input camera readings and process this
JalilChavez 1:a457798861a4 16 * information in order to control direction and speed.
JalilChavez 1:a457798861a4 17 * Rev 0.1 14/10/2014
JalilChavez 1:a457798861a4 18 * - Added support of internal accelerometer.
JalilChavez 1:a457798861a4 19 * - Code commented.
JalilChavez 1:a457798861a4 20 * - Included Rx and Tx Buffer in order to allow MSGHDL integration.
JalilChavez 2:c878e4203b62 21 * Rev 0.1.1 14/10/2014
JalilChavez 2:c878e4203b62 22 * - Corrected placing o data into Tx buffer
josepesado 3:7bdacc4cf273 23 * Rev 0.2 18/10/2014
josepesado 3:7bdacc4cf273 24 * - Added XBee services.
JalilChavez 1:a457798861a4 25 */
JalilChavez 1:a457798861a4 26
JalilChavez 1:a457798861a4 27 /**
JalilChavez 1:a457798861a4 28 * Header files
JalilChavez 1:a457798861a4 29 */
JalilChavez 1:a457798861a4 30 #include "camctl.h" /* Camera controller */
JalilChavez 1:a457798861a4 31 #include "srvctl.h" /* Servo controller */
JalilChavez 1:a457798861a4 32 #include "mtrctl.h" /* Motor controller */
josepesado 3:7bdacc4cf273 33 #include "xbee.h" /* XBee controller */
JalilChavez 1:a457798861a4 34 #include "FXOS8700CQ.h" /* Accelerometer controller */
JalilChavez 0:2a825db40e1b 35
JalilChavez 1:a457798861a4 36 /**
JalilChavez 1:a457798861a4 37 * Object initialization
JalilChavez 1:a457798861a4 38 */
JalilChavez 0:2a825db40e1b 39 DigitalOut MAIN__xPinAlive( LED_BLUE );
JalilChavez 1:a457798861a4 40 PwmOut MAIN__xLEDIllumination( D3 );
JalilChavez 1:a457798861a4 41 FXOS8700CQ IMU( PTE25 /*SDA*/, PTE24 /*SCL*/, FXOS8700CQ_SLAVE_ADDR1/*ADDRESS*/);
JalilChavez 0:2a825db40e1b 42 Serial xComPort1(USBTX,USBRX);
JalilChavez 0:2a825db40e1b 43
JalilChavez 1:a457798861a4 44 /**
JalilChavez 1:a457798861a4 45 * Global variable declaration
JalilChavez 1:a457798861a4 46 */
JalilChavez 0:2a825db40e1b 47 uint16_t au16ThresholdBuffer[BUFFER_LENGTH] = {0};
JalilChavez 0:2a825db40e1b 48 uint16_t *pu16DataBuffer;
JalilChavez 0:2a825db40e1b 49 uint8_t u8Index = 0;
JalilChavez 0:2a825db40e1b 50 uint8_t u8FrameCtr = 0;
JalilChavez 0:2a825db40e1b 51 /* Variables to get the line center */
JalilChavez 0:2a825db40e1b 52 uint32_t u32Sum = 0;
JalilChavez 1:a457798861a4 53 uint8_t u8LinePos = 0;
JalilChavez 0:2a825db40e1b 54 uint32_t u3SampleCtr = 0;
JalilChavez 0:2a825db40e1b 55 bool boCalibrationDone = false;
JalilChavez 1:a457798861a4 56 /* Accelerometer variables */
JalilChavez 1:a457798861a4 57 uint8_t u8IMUStatus = 0;
JalilChavez 1:a457798861a4 58 SRAWDATA accData;
JalilChavez 1:a457798861a4 59 SRAWDATA magData;
JalilChavez 1:a457798861a4 60 /* TxBuffer and RxBuffer */
JalilChavez 1:a457798861a4 61 uint8_t au8TxBuffer[9] = {0};
JalilChavez 1:a457798861a4 62 uint8_t au8RxBuffer[9] = {0};
JalilChavez 1:a457798861a4 63 /*
JalilChavez 1:a457798861a4 64 * Element0 --> u8LinePos
JalilChavez 1:a457798861a4 65 * Element1 --> u8AccData.x
JalilChavez 1:a457798861a4 66 * Element3 --> u8AccData.y
JalilChavez 1:a457798861a4 67 */
JalilChavez 0:2a825db40e1b 68
JalilChavez 1:a457798861a4 69
JalilChavez 1:a457798861a4 70 /**
JalilChavez 1:a457798861a4 71 * Main program
JalilChavez 1:a457798861a4 72 */
JalilChavez 0:2a825db40e1b 73 int main()
JalilChavez 0:2a825db40e1b 74 {
JalilChavez 0:2a825db40e1b 75 /* Initialization of modules */
JalilChavez 0:2a825db40e1b 76 CAMCTL_vInit();
JalilChavez 0:2a825db40e1b 77 MTRCTL_vInit();
JalilChavez 0:2a825db40e1b 78 SRVCTL_vInit();
josepesado 3:7bdacc4cf273 79 XBee_vInit();
JalilChavez 1:a457798861a4 80 IMU.enable();
JalilChavez 1:a457798861a4 81 MAIN__xLEDIllumination.period_us(8333);
JalilChavez 0:2a825db40e1b 82 xComPort1.baud(115200);
JalilChavez 1:a457798861a4 83
JalilChavez 1:a457798861a4 84 /**
JalilChavez 1:a457798861a4 85 * Main loop
JalilChavez 1:a457798861a4 86 */
JalilChavez 0:2a825db40e1b 87 while(true)
JalilChavez 0:2a825db40e1b 88 {
JalilChavez 0:2a825db40e1b 89 /* Wait until camera is calibrated */
JalilChavez 0:2a825db40e1b 90 if(boCalibrationDone == false)
JalilChavez 0:2a825db40e1b 91 {
JalilChavez 0:2a825db40e1b 92 boCalibrationDone = CAMCTL_boCalibrateCamera( au16ThresholdBuffer, 4 );
JalilChavez 0:2a825db40e1b 93 }
JalilChavez 0:2a825db40e1b 94 else
JalilChavez 0:2a825db40e1b 95 {
JalilChavez 1:a457798861a4 96 MAIN__xLEDIllumination = 1.0;
JalilChavez 0:2a825db40e1b 97 /* Get frame */
JalilChavez 0:2a825db40e1b 98 CAMCTL_vTriggerOneShotCapture();
JalilChavez 0:2a825db40e1b 99 pu16DataBuffer = CAMCTL_pu16GetData();
JalilChavez 0:2a825db40e1b 100 while( pu16DataBuffer == NULL )
JalilChavez 0:2a825db40e1b 101 {
JalilChavez 0:2a825db40e1b 102 pu16DataBuffer = CAMCTL_pu16GetData();
JalilChavez 0:2a825db40e1b 103 }
JalilChavez 0:2a825db40e1b 104 /* Detect path center */
JalilChavez 1:a457798861a4 105 for( u8Index = 0; u8Index < (BUFFER_LENGTH); u8Index++ )
JalilChavez 0:2a825db40e1b 106 {
JalilChavez 0:2a825db40e1b 107 if( pu16DataBuffer[u8Index] < au16ThresholdBuffer[u8Index] )
JalilChavez 0:2a825db40e1b 108 {
JalilChavez 0:2a825db40e1b 109 u32Sum = u32Sum + u8Index;
JalilChavez 0:2a825db40e1b 110 u3SampleCtr++;
JalilChavez 0:2a825db40e1b 111
JalilChavez 0:2a825db40e1b 112 }
JalilChavez 0:2a825db40e1b 113 else
JalilChavez 0:2a825db40e1b 114 {
JalilChavez 0:2a825db40e1b 115
JalilChavez 0:2a825db40e1b 116 }
JalilChavez 0:2a825db40e1b 117 }
JalilChavez 0:2a825db40e1b 118 /*Move servo according to camer readings*/
JalilChavez 1:a457798861a4 119 u8LinePos = (uint8_t)( u32Sum / u3SampleCtr );
JalilChavez 1:a457798861a4 120 if( u3SampleCtr > 10 )
JalilChavez 1:a457798861a4 121 {
JalilChavez 1:a457798861a4 122 SRVCTL_vSetPosition( (uint16_t)(BUFFER_LENGTH-u8LinePos), BUFFER_LENGTH );
JalilChavez 1:a457798861a4 123 MTRCTL_vSetSystemSpeed( 1000, (uint8_t)(BUFFER_LENGTH-u8LinePos) );
JalilChavez 1:a457798861a4 124 /* Report data through COM port */
JalilChavez 1:a457798861a4 125 xComPort1.printf("CAM: Line position = %u\r\n",u8LinePos);
JalilChavez 1:a457798861a4 126 /* Place data in Tx buffer */
JalilChavez 1:a457798861a4 127 au8TxBuffer[0] = u8LinePos;
JalilChavez 1:a457798861a4 128
JalilChavez 1:a457798861a4 129 }
JalilChavez 1:a457798861a4 130 else
JalilChavez 1:a457798861a4 131 {
JalilChavez 1:a457798861a4 132 xComPort1.printf("CAM: Readings not valid\r\n");
JalilChavez 1:a457798861a4 133 }
JalilChavez 0:2a825db40e1b 134 /* Reinitialize variables */
JalilChavez 0:2a825db40e1b 135 u32Sum = 0;
JalilChavez 0:2a825db40e1b 136 u3SampleCtr = 0;
JalilChavez 0:2a825db40e1b 137 }
JalilChavez 1:a457798861a4 138 u8IMUStatus = IMU.get_data(&accData,&magData);
JalilChavez 1:a457798861a4 139 if( u8IMUStatus == 0 )
JalilChavez 1:a457798861a4 140 {
JalilChavez 1:a457798861a4 141 /* Place data in Tx Buffer */
JalilChavez 2:c878e4203b62 142 au8TxBuffer[1] = (uint8_t)((accData.x)&0x00FF);
JalilChavez 2:c878e4203b62 143 au8TxBuffer[2] = (uint8_t)(((accData.x)&0xFF00)>>8);
JalilChavez 2:c878e4203b62 144 au8TxBuffer[3] = (uint8_t)((accData.y)&0x00FF);
JalilChavez 2:c878e4203b62 145 au8TxBuffer[4] = (uint8_t)(((accData.y)&0xFF00)>>8);
JalilChavez 1:a457798861a4 146 xComPort1.printf("ACCEL: %i,%i \r\n",accData.x,accData.y);
JalilChavez 1:a457798861a4 147 }
JalilChavez 1:a457798861a4 148 else
JalilChavez 1:a457798861a4 149 {
JalilChavez 1:a457798861a4 150 xComPort1.printf("ACCEL: Data invalid\r\n");
JalilChavez 1:a457798861a4 151 }
josepesado 3:7bdacc4cf273 152
josepesado 3:7bdacc4cf273 153 memcpy(CommandTx,au8TxBuffer,9);
josepesado 3:7bdacc4cf273 154 XBee_u8TxRq = 1;
josepesado 3:7bdacc4cf273 155
josepesado 3:7bdacc4cf273 156 XBee_vMain();
josepesado 3:7bdacc4cf273 157
JalilChavez 1:a457798861a4 158 /* Pin alive indicator */
JalilChavez 0:2a825db40e1b 159 MAIN__xPinAlive = !MAIN__xPinAlive;
JalilChavez 0:2a825db40e1b 160 wait(0.0001);
JalilChavez 0:2a825db40e1b 161 }
JalilChavez 0:2a825db40e1b 162 }
JalilChavez 0:2a825db40e1b 163