/* * Copyright 2016-2020 NXP * All rights reserved. * * Redistribution and use in source and binary forms, with or without modification, * are permitted provided that the following conditions are met: * * o Redistributions of source code must retain the above copyright notice, this list * of conditions and the following disclaimer. * * o Redistributions in binary form must reproduce the above copyright notice, this * list of conditions and the following disclaimer in the documentation and/or * other materials provided with the distribution. * * o Neither the name of NXP Semiconductor, Inc. nor the names of its * contributors may be used to endorse or promote products derived from this * software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ /** * @file nxpcup.cpp * @brief Application entry point. */ #include #include "board.h" #include "peripherals.h" #include "pin_mux.h" #include "clock_config.h" #include "MK64F12.h" #include "fsl_debug_console.h" #include "tfc.h" class PID { private: const float kP, kD, kI, maxkI; float sum = 0, lastErr = 0; public: int calculate(float error, float frequency) { sum += error*kI/frequency; if(sum>maxkI) { sum = maxkI; } else if(sum<-maxkI) { sum = -maxkI; } float deriv = (lastErr - error)*frequency; float toReturn = error*kP + deriv*kD + sum; lastErr = error; return toReturn; } PID(float kP, float kD, float kI, float maxkI) : kP(kP), kD(kD), kI(kI), maxkI(maxkI) {}; }; PID pidMotors(3.5f, 0, 0, 1000); // PID which is used to control motors void setMotorsSpeed(float desiredSpeed) { int pwmSpeed = pidMotors.calculate(desiredSpeed-Encoder::currentSpeed, Encoder::readsPerSecond); if(pwmSpeed < -1000) pwmSpeed = -1000; if(pwmSpeed > 1000) pwmSpeed = 1000; Motor::setLMotorSpeed(pwmSpeed); Motor::setRMotorSpeed(pwmSpeed); } float desiredSpeed = 0; void onEncoderUpdate() { // function which is called from tfc.h when there are new encoder results setMotorsSpeed(desiredSpeed); } char smer = 0; char wrongSmer = 0; unsigned short lastPos = 0; struct Line { char pos; char thick; }; struct Lines { struct Line lines[32]; int linesCount; }; void getLinesByDerivation(unsigned short* image, struct Lines* lines) { // get all lines from image short lineBegin = -1; char startOfLineFound = 0; for(int i=4; i<125; i++) { // we apply derivation on all parts of image and if values are high enough, then we store it as line short derivated = (short)image[i-4] + (short)image[i-3] + (short)image[i-2] + (short)image[i-1] - (short)image[i+0] - (short)image[i+1] - (short)image[i+2] - (short)image[i+3]; if(derivated<-150 && startOfLineFound) { if(lineBegin == -1) lineBegin = 4; short linePos = lineBegin + ((i - lineBegin) >> 1); lines->lines[lines->linesCount].pos = linePos; lines->lines[lines->linesCount].thick = ((i - lineBegin) >> 1); lines->linesCount++; startOfLineFound = 0; } else if(derivated>150) { startOfLineFound = 1; lineBegin = i; } } } void getLinesPos(struct Lines *lines, short *left, short *right) { // find left and right line, used on boot *left = -1; *right = -1; for(int i=0; ilinesCount; i++) { if(lines->lines[i].pos<64 && *left == -1) { // check if line is in left part of image and is first left line *left = i; } if(lines->lines[i].pos>=64) { // check if line is in right part of image *right = i; } } } short getClosestLine(struct Lines *lines, short pos) { // get closest line to given position short ret = -999; short lastDistance = 999; for(int i=0; ilinesCount; i++) { short newDistance = lines->lines[i].pos - pos; if(newDistance<0) newDistance = - newDistance; if(newDistancelines[i].pos; lastDistance = newDistance; } } if(ret==-999) return -1; return ret; } short leftLine = -1; short rightLine = -1; /* * @brief Application entry point. */ int main(void) { /* Init board hardware. */ BOARD_InitBootPins(); BOARD_InitBootClocks(); BOARD_InitBootPeripherals(); /* Init FSL debug console. */ BOARD_InitDebugConsole(); PRINTF("Hello World\n"); // this is here to check if USB serial is working setupTFC(); // setup whole main board and it's components cameraInitialCalibration(); // here we calibrate camera while(1) { if(UART3->RCFIFO != 0U) { // here we async check if there any received data from BT char ch = UART3->D; // if so, then we read those data switch(ch) { // we set speed based on data case 'a': desiredSpeed = 40; break; case 'b': desiredSpeed = 20; break; case 'c': desiredSpeed = 0; break; } UART0->D = ch; // here we clone data to USB serial UART3->D = ch; // and here we clone data to BT serial } unsigned short* image = getNextImage(); // here we try to get next image from camera if(!image) continue; // if we havent got image, we will try it next time struct Lines lines = {0}; getLinesByDerivation(image, &lines); if(leftLine == -1 || rightLine == -1) { // if line positions are unset, we try to get them getLinesPos(&lines, &leftLine, &rightLine); } else { // we search based on previous line positions if(lines.linesCount==0) { // if we hadn't found any lines, then we set servo to center setServoPos(0); continue; } short newLeft = getClosestLine(&lines, leftLine); // we get closest line to left line short newRight = getClosestLine(&lines, rightLine); // we get closest line to right line if(newLeft == newRight) { // if the lines are same, then we choose to which line will the new line be set short newLeftDistance = newLeft - leftLine; short newRightDistance = newRight - rightLine; if(newLeftDistance<0) newLeftDistance = -newLeftDistance; if(newRightDistance<0) newRightDistance = -newRightDistance; if(newLeftDistance70) { // make sure that lines are far enough from themself leftLine = newLeft; rightLine = newRight; } else { // if they arent then, pick the one which is closest to its previous position if(newLeftDistance 100) middlePos = 100; setServoPos(-middlePos); } } } return 0; }