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