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@1:a457798861a4, 2014-10-17 (annotated)

Committer:
JalilChavez
Date:
Fri Oct 17 21:50:24 2014 +0000
Revision:
1:a457798861a4
Parent:
0:2a825db40e1b
Child:
2:c878e4203b62
- Added support for internal accelerometer.; - Code commented.; - Included Tx and Rx buffers.

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 1:a457798861a4 21 */
JalilChavez 1:a457798861a4 22
JalilChavez 1:a457798861a4 23 /**
JalilChavez 1:a457798861a4 24 * Header files
JalilChavez 1:a457798861a4 25 */
JalilChavez 1:a457798861a4 26 #include "camctl.h" /* Camera controller */
JalilChavez 1:a457798861a4 27 #include "srvctl.h" /* Servo controller */
JalilChavez 1:a457798861a4 28 #include "mtrctl.h" /* Motor controller */
JalilChavez 1:a457798861a4 29 #include "FXOS8700CQ.h" /* Accelerometer controller */
JalilChavez 0:2a825db40e1b 30
JalilChavez 1:a457798861a4 31 /**
JalilChavez 1:a457798861a4 32 * Object initialization
JalilChavez 1:a457798861a4 33 */
JalilChavez 0:2a825db40e1b 34 DigitalOut MAIN__xPinAlive( LED_BLUE );
JalilChavez 1:a457798861a4 35 PwmOut MAIN__xLEDIllumination( D3 );
JalilChavez 1:a457798861a4 36 FXOS8700CQ IMU( PTE25 /*SDA*/, PTE24 /*SCL*/, FXOS8700CQ_SLAVE_ADDR1/*ADDRESS*/);
JalilChavez 0:2a825db40e1b 37 Serial xComPort1(USBTX,USBRX);
JalilChavez 0:2a825db40e1b 38
JalilChavez 1:a457798861a4 39 /**
JalilChavez 1:a457798861a4 40 * Global variable declaration
JalilChavez 1:a457798861a4 41 */
JalilChavez 0:2a825db40e1b 42 uint16_t au16ThresholdBuffer[BUFFER_LENGTH] = {0};
JalilChavez 0:2a825db40e1b 43 uint16_t *pu16DataBuffer;
JalilChavez 0:2a825db40e1b 44 uint8_t u8Index = 0;
JalilChavez 0:2a825db40e1b 45 uint8_t u8FrameCtr = 0;
JalilChavez 0:2a825db40e1b 46 /* Variables to get the line center */
JalilChavez 0:2a825db40e1b 47 uint32_t u32Sum = 0;
JalilChavez 1:a457798861a4 48 uint8_t u8LinePos = 0;
JalilChavez 0:2a825db40e1b 49 uint32_t u3SampleCtr = 0;
JalilChavez 0:2a825db40e1b 50 bool boCalibrationDone = false;
JalilChavez 1:a457798861a4 51 /* Accelerometer variables */
JalilChavez 1:a457798861a4 52 uint8_t u8IMUStatus = 0;
JalilChavez 1:a457798861a4 53 SRAWDATA accData;
JalilChavez 1:a457798861a4 54 SRAWDATA magData;
JalilChavez 1:a457798861a4 55 /* TxBuffer and RxBuffer */
JalilChavez 1:a457798861a4 56 uint8_t au8TxBuffer[9] = {0};
JalilChavez 1:a457798861a4 57 uint8_t au8RxBuffer[9] = {0};
JalilChavez 1:a457798861a4 58 /*
JalilChavez 1:a457798861a4 59 * Element0 --> u8LinePos
JalilChavez 1:a457798861a4 60 * Element1 --> u8AccData.x
JalilChavez 1:a457798861a4 61 * Element3 --> u8AccData.y
JalilChavez 1:a457798861a4 62 */
JalilChavez 0:2a825db40e1b 63
JalilChavez 1:a457798861a4 64
JalilChavez 1:a457798861a4 65 /**
JalilChavez 1:a457798861a4 66 * Main program
JalilChavez 1:a457798861a4 67 */
JalilChavez 0:2a825db40e1b 68 int main()
JalilChavez 0:2a825db40e1b 69 {
JalilChavez 0:2a825db40e1b 70 /* Initialization of modules */
JalilChavez 0:2a825db40e1b 71 CAMCTL_vInit();
JalilChavez 0:2a825db40e1b 72 MTRCTL_vInit();
JalilChavez 0:2a825db40e1b 73 SRVCTL_vInit();
JalilChavez 1:a457798861a4 74 IMU.enable();
JalilChavez 1:a457798861a4 75 MAIN__xLEDIllumination.period_us(8333);
JalilChavez 0:2a825db40e1b 76 xComPort1.baud(115200);
JalilChavez 1:a457798861a4 77
JalilChavez 1:a457798861a4 78 /**
JalilChavez 1:a457798861a4 79 * Main loop
JalilChavez 1:a457798861a4 80 */
JalilChavez 0:2a825db40e1b 81 while(true)
JalilChavez 0:2a825db40e1b 82 {
JalilChavez 0:2a825db40e1b 83 /* Wait until camera is calibrated */
JalilChavez 0:2a825db40e1b 84 if(boCalibrationDone == false)
JalilChavez 0:2a825db40e1b 85 {
JalilChavez 0:2a825db40e1b 86 boCalibrationDone = CAMCTL_boCalibrateCamera( au16ThresholdBuffer, 4 );
JalilChavez 0:2a825db40e1b 87 }
JalilChavez 0:2a825db40e1b 88 else
JalilChavez 0:2a825db40e1b 89 {
JalilChavez 1:a457798861a4 90 MAIN__xLEDIllumination = 1.0;
JalilChavez 0:2a825db40e1b 91 /* Get frame */
JalilChavez 0:2a825db40e1b 92 CAMCTL_vTriggerOneShotCapture();
JalilChavez 0:2a825db40e1b 93 pu16DataBuffer = CAMCTL_pu16GetData();
JalilChavez 0:2a825db40e1b 94 while( pu16DataBuffer == NULL )
JalilChavez 0:2a825db40e1b 95 {
JalilChavez 0:2a825db40e1b 96 pu16DataBuffer = CAMCTL_pu16GetData();
JalilChavez 0:2a825db40e1b 97 }
JalilChavez 0:2a825db40e1b 98 /* Detect path center */
JalilChavez 1:a457798861a4 99 for( u8Index = 0; u8Index < (BUFFER_LENGTH); u8Index++ )
JalilChavez 0:2a825db40e1b 100 {
JalilChavez 0:2a825db40e1b 101 if( pu16DataBuffer[u8Index] < au16ThresholdBuffer[u8Index] )
JalilChavez 0:2a825db40e1b 102 {
JalilChavez 0:2a825db40e1b 103 u32Sum = u32Sum + u8Index;
JalilChavez 0:2a825db40e1b 104 u3SampleCtr++;
JalilChavez 0:2a825db40e1b 105
JalilChavez 0:2a825db40e1b 106 }
JalilChavez 0:2a825db40e1b 107 else
JalilChavez 0:2a825db40e1b 108 {
JalilChavez 0:2a825db40e1b 109
JalilChavez 0:2a825db40e1b 110 }
JalilChavez 0:2a825db40e1b 111 }
JalilChavez 0:2a825db40e1b 112 /*Move servo according to camer readings*/
JalilChavez 1:a457798861a4 113 u8LinePos = (uint8_t)( u32Sum / u3SampleCtr );
JalilChavez 1:a457798861a4 114 if( u3SampleCtr > 10 )
JalilChavez 1:a457798861a4 115 {
JalilChavez 1:a457798861a4 116 SRVCTL_vSetPosition( (uint16_t)(BUFFER_LENGTH-u8LinePos), BUFFER_LENGTH );
JalilChavez 1:a457798861a4 117 MTRCTL_vSetSystemSpeed( 1000, (uint8_t)(BUFFER_LENGTH-u8LinePos) );
JalilChavez 1:a457798861a4 118 /* Report data through COM port */
JalilChavez 1:a457798861a4 119 xComPort1.printf("CAM: Line position = %u\r\n",u8LinePos);
JalilChavez 1:a457798861a4 120 /* Place data in Tx buffer */
JalilChavez 1:a457798861a4 121 au8TxBuffer[0] = u8LinePos;
JalilChavez 1:a457798861a4 122
JalilChavez 1:a457798861a4 123 }
JalilChavez 1:a457798861a4 124 else
JalilChavez 1:a457798861a4 125 {
JalilChavez 1:a457798861a4 126 xComPort1.printf("CAM: Readings not valid\r\n");
JalilChavez 1:a457798861a4 127 }
JalilChavez 0:2a825db40e1b 128 /* Reinitialize variables */
JalilChavez 0:2a825db40e1b 129 u32Sum = 0;
JalilChavez 0:2a825db40e1b 130 u3SampleCtr = 0;
JalilChavez 0:2a825db40e1b 131 }
JalilChavez 1:a457798861a4 132 u8IMUStatus = IMU.get_data(&accData,&magData);
JalilChavez 1:a457798861a4 133 if( u8IMUStatus == 0 )
JalilChavez 1:a457798861a4 134 {
JalilChavez 1:a457798861a4 135 /* Place data in Tx Buffer */
JalilChavez 1:a457798861a4 136 au8TxBuffer[1] = accData.x;
JalilChavez 1:a457798861a4 137 au8TxBuffer[3] = accData.y;
JalilChavez 1:a457798861a4 138 xComPort1.printf("ACCEL: %i,%i \r\n",accData.x,accData.y);
JalilChavez 1:a457798861a4 139 }
JalilChavez 1:a457798861a4 140 else
JalilChavez 1:a457798861a4 141 {
JalilChavez 1:a457798861a4 142 xComPort1.printf("ACCEL: Data invalid\r\n");
JalilChavez 1:a457798861a4 143 }
JalilChavez 1:a457798861a4 144 /* Pin alive indicator */
JalilChavez 0:2a825db40e1b 145 MAIN__xPinAlive = !MAIN__xPinAlive;
JalilChavez 0:2a825db40e1b 146 wait(0.0001);
JalilChavez 0:2a825db40e1b 147 }
JalilChavez 0:2a825db40e1b 148 }
JalilChavez 0:2a825db40e1b 149