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@3:7bdacc4cf273, 2014-10-18 (annotated)
- Committer:
- josepesado
- Date:
- Sat Oct 18 13:01:47 2014 +0000
- Revision:
- 3:7bdacc4cf273
- Parent:
- 2:c878e4203b62
Master Vechicle with RF communications
Who changed what in which revision?
User | Revision | Line number | New 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 |