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

Committer:
JalilChavez
Date:
Fri Oct 17 22:44:17 2014 +0000
Revision:
2:c878e4203b62
Parent:
1:a457798861a4
Child:
3:7bdacc4cf273
Rev 0.1.1 14/10/2014;  - Corrected placing o data into Tx buffer

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