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?
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 | 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 |