2020 NXP Cup Innovation Challenge contest -Autonomous Racing Car

/* * 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 <stdio.h> #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; i<lines->linesCount; 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; i<lines->linesCount; i++) { short newDistance = lines->lines[i].pos - pos; if(newDistance<0) newDistance = - newDistance; if(newDistance<lastDistance) { ret = lines->lines[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(newLeftDistance<newRightDistance && newLeft != -1) { leftLine = newLeft; } else if(newRightDistance<newLeftDistance && newRight != -1) { rightLine = newRight; } } else { // if new lines are different short newLeftDistance = newLeft - leftLine; // calculate line distance form previous position short newRightDistance = newRight - rightLine; // calculate line distance form previous position short linesDistance = newRight - newLeft; // calculate distance between left and right line if(newLeftDistance<0) newLeftDistance = -newLeftDistance; // get absolute value if(newRightDistance<0) newRightDistance = -newRightDistance; // get absolute value if(linesDistance>70) { // 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<newRightDistance) { leftLine = newLeft; } else if(newRightDistance<newLeftDistance) { rightLine = newRight; } } } if(leftLine!=-1 && rightLine !=-1) { // if both lines are set, then we set servo to position based on them short middlePos = leftLine + rightLine; middlePos -= 128; // by this we make middle value 0 middlePos = (middlePos*4.0f); if(middlePos < -100 ) middlePos = -100; if(middlePos > 100) middlePos = 100; setServoPos(-middlePos); } } } return 0; }

#ifndef __TFC_H__ #define __TFC_H__ #include "fsl_ftm.h" #include "fsl_i2c.h" #include "fsl_adc16.h" #include "fsl_pit.h" #include "fsl_uart.h" #include "fsl_dspi.h" #define SERVO_MODULO 9375 // (ftmClock / pwmFreq_Hz) - 1 volatile unsigned int servoPos = 0; // access this variable to set servo pos volatile unsigned int laserPos = 655; void setupServo(int startupVal) { servoPos = 700 + startupVal; ftm_config_t ftmConfig; FTM_GetDefaultConfig(&ftmConfig); ftmConfig.prescale = kFTM_Prescale_Divide_128; FTM_Init(FTM0, &ftmConfig); FTM0->SC &= ~FTM_SC_CPWMS_MASK; FTM0->MOD = SERVO_MODULO; /* Clear the current mode and edge level bits */ uint32_t reg = FTM0->CONTROLS[1].CnSC; reg &= ~(FTM_CnSC_MSA_MASK | FTM_CnSC_MSB_MASK | FTM_CnSC_ELSA_MASK | FTM_CnSC_ELSB_MASK); /* Setup the active level */ reg |= (uint32_t)kFTM_HighTrue << FTM_CnSC_ELSA_SHIFT; /* Edge-aligned mode needs MSB to be 1, don't care for Center-aligned mode */ reg |= FTM_CnSC_MSB(1U); /* Update the mode and edge level */ FTM0->CONTROLS[1].CnSC = reg; FTM0->CONTROLS[1].CnV = servoPos; /* Clear the current mode and edge level bits */ reg = FTM0->CONTROLS[2].CnSC; reg &= ~(FTM_CnSC_MSA_MASK | FTM_CnSC_MSB_MASK | FTM_CnSC_ELSA_MASK | FTM_CnSC_ELSB_MASK); /* Setup the active level */ reg |= (uint32_t)kFTM_HighTrue << FTM_CnSC_ELSA_SHIFT; /* Edge-aligned mode needs MSB to be 1, don't care for Center-aligned mode */ reg |= FTM_CnSC_MSB(1U); /* Update the mode and edge level */ FTM0->CONTROLS[2].CnSC = reg; FTM0->CONTROLS[2].CnV = servoPos; FTM0->SC |= FTM_SC_TOIE_MASK; // enable FTM0 overflow iterrupt PORTC->PCR[2] = PORT_PCR_MUX(4); PORTC->PCR[3] = PORT_PCR_MUX(4); FTM_StartTimer(FTM0, kFTM_SystemClock); NVIC_EnableIRQ(FTM0_IRQn); FTM_EnableInterrupts(FTM0, kFTM_TimeOverflowInterruptEnable); } char getLaserState() { // TODO return 0; } void setServoPos(int pos) { servoPos = 700 + pos; } void setLaserPos(unsigned int pos) { laserPos = pos; } /*extern */volatile short noSee; char isLaserDecreasing = 0; char isLaserSearching = 0; volatile char isObstacleUpdated = 0; volatile int obstaclePos = 0; volatile int endObstaclePos = 0; volatile int obstacleSize = 0; volatile char isObtacleHere = 0; char __laserColisonStarted = 0; int __laserColisionBegin = 0; extern "C" { void FTM0_IRQHandler(void) { if(!(FTM0->SC&FTM_SC_TOF_MASK)) return; FTM0->SC &= ~(FTM_SC_TOF_MASK); FTM0->CONTROLS[1].CnV = servoPos; if(isLaserSearching) { char currentLaserState = getLaserState(); if(!__laserColisonStarted && currentLaserState) { __laserColisonStarted = 1; __laserColisionBegin = laserPos; isObtacleHere = 1; } else if(__laserColisonStarted && !currentLaserState) { __laserColisonStarted = 0; isLaserDecreasing = !isLaserDecreasing; isObstacleUpdated = 1; isObtacleHere = 1; obstacleSize = __laserColisionBegin - laserPos; if(obstacleSize<0) { obstacleSize = -obstacleSize; } obstacleSize >>= 1; //obstaclePos = __laserColisionBegin + laserPos; int middleDistanceOld = __laserColisionBegin - 543; int middleDistanceNew = laserPos - 543; if(middleDistanceOld<0) middleDistanceOld = -middleDistanceOld; if(middleDistanceNew<0) middleDistanceNew = -middleDistanceNew; if(middleDistanceOld<middleDistanceNew) { obstaclePos = __laserColisionBegin<<1; endObstaclePos = laserPos; } else { obstaclePos = laserPos<<1; endObstaclePos = __laserColisionBegin; } obstaclePos >>= 1; obstaclePos -= 543; // based on servo middle obstaclePos = -obstaclePos; endObstaclePos -= 543; // based on servo middle endObstaclePos = -obstaclePos; } if(!isLaserDecreasing) { if(__laserColisonStarted) { laserPos += 20; } else { laserPos += 3; //30; } } else { if(__laserColisonStarted) { laserPos -= 20; } else { laserPos -= 3; //30; } } // https://servodatabase.com/servo/towerpro/sg90 // 0,5ms to 2,5ms if(laserPos<=187) { // 187 isLaserDecreasing = 0; if(__laserColisonStarted) { __laserColisonStarted = 0; isObstacleUpdated = 1; isObtacleHere = 1; obstacleSize = __laserColisionBegin - laserPos; if(obstacleSize<0) { obstacleSize = -obstacleSize; } obstacleSize >>= 1; //obstaclePos = __laserColisionBegin + laserPos; int middleDistanceOld = __laserColisionBegin - 543; int middleDistanceNew = laserPos - 543; if(middleDistanceOld<0) middleDistanceOld = -middleDistanceOld; if(middleDistanceNew<0) middleDistanceNew = -middleDistanceNew; if(middleDistanceOld<middleDistanceNew) { obstaclePos = __laserColisionBegin<<1; endObstaclePos = laserPos; } else { obstaclePos = laserPos<<1; endObstaclePos = __laserColisionBegin; } obstaclePos >>= 1; obstaclePos -= 543; // based on servo middle obstaclePos = -obstaclePos; endObstaclePos -= 543; // based on servo middle endObstaclePos = -obstaclePos; } else { isObtacleHere = 0; obstacleSize = 0; obstaclePos = 0; endObstaclePos = 0; } } else if(laserPos>=900) { // 938 isLaserDecreasing = 1; if(__laserColisonStarted) { __laserColisonStarted = 0; isObstacleUpdated = 1; isObtacleHere = 1; obstacleSize = __laserColisionBegin - laserPos; if(obstacleSize<0) { obstacleSize = -obstacleSize; } obstacleSize>>=1; //obstaclePos = __laserColisionBegin + laserPos; int middleDistanceOld = __laserColisionBegin - 543; int middleDistanceNew = laserPos - 543; if(middleDistanceOld<0) middleDistanceOld = -middleDistanceOld; if(middleDistanceNew<0) middleDistanceNew = -middleDistanceNew; if(middleDistanceOld<middleDistanceNew) { obstaclePos = __laserColisionBegin<<1; endObstaclePos = laserPos; } else { obstaclePos = laserPos<<1; endObstaclePos = __laserColisionBegin; } obstaclePos >>= 1; obstaclePos -= 543; // based on servo middle obstaclePos = -obstaclePos; endObstaclePos -= 543; // based on servo middle endObstaclePos = -obstaclePos; } else { isObtacleHere = 0; obstacleSize = 0; obstaclePos = 0; endObstaclePos = 0; } } } FTM0->CONTROLS[2].CnV = laserPos; // we set servo at right time to prevent bad output FTM0->SYNC |= 1 << 7; // trigger SW sync noSee++; } } namespace Motor { const unsigned int MOTOR_MODULO = 1000; volatile int rMotorSpeed = 0; volatile int lMotorSpeed = 0; void initMotorChannel(int channel) { /* Clear the current mode and edge level bits */ uint32_t reg = FTM3->CONTROLS[channel].CnSC; reg &= ~(FTM_CnSC_MSA_MASK | FTM_CnSC_MSB_MASK | FTM_CnSC_ELSA_MASK | FTM_CnSC_ELSB_MASK); /* Setup the active level */ reg |= (uint32_t)(kFTM_HighTrue << FTM_CnSC_ELSA_SHIFT); /* Edge-aligned mode needs MSB to be 1, don't care for Center-aligned mode */ reg |= FTM_CnSC_MSB(1U); /* Update the mode and edge level */ FTM3->CONTROLS[channel].CnSC = reg; } void setup() { ftm_config_t ftmConfig; FTM_GetDefaultConfig(&ftmConfig); ftmConfig.prescale = kFTM_Prescale_Divide_64; FTM_Init(FTM3, &ftmConfig); FTM3->SC &= ~FTM_SC_CPWMS_MASK; FTM3->MOD = MOTOR_MODULO; // MOTOR R: ch4, ch5 // MOTOR L: ch1, ch0 initMotorChannel(0); initMotorChannel(1); initMotorChannel(4); initMotorChannel(5); ftm_chnl_pwm_signal_param_t pwmPar; pwmPar.chnlNumber = kFTM_Chnl_0; pwmPar.dutyCyclePercent = 0; pwmPar.enableDeadtime = false; pwmPar.firstEdgeDelayPercent = 0; pwmPar.level = kFTM_HighTrue; FTM_SetupPwm(FTM3, &pwmPar, 1, kFTM_EdgeAlignedPwm, 1000, 60000000); //int startupVal = MOTOR_MODULO >> 1; // MOTOR_MODULO / 2 for ability of back riding; 0 for forward only; MOTOR_MODULO for backward only FTM3->CONTROLS[1].CnV = 0; FTM3->CONTROLS[0].CnV = 0; FTM3->CONTROLS[4].CnV = 0; FTM3->CONTROLS[5].CnV = 0; FTM_StartTimer(FTM3, kFTM_SystemClock); PORTD->PCR[0] = PORT_PCR_MUX(4); PORTD->PCR[1] = PORT_PCR_MUX(4); PORTC->PCR[8] = PORT_PCR_MUX(3); PORTC->PCR[9] = PORT_PCR_MUX(3); FTM_StartTimer(FTM3, kFTM_SystemClock); NVIC_EnableIRQ(FTM3_IRQn); FTM_EnableInterrupts(FTM3, kFTM_TimeOverflowInterruptEnable); } inline void setLMotorSpeed(int speed) { lMotorSpeed = speed; } inline void setRMotorSpeed(int speed) { rMotorSpeed = speed; } }; extern "C" { void FTM3_IRQHandler() { // we need FTM3 interrupt, to update CnV to fix double update if(!(FTM3->SC&FTM_SC_TOF_MASK)) return; FTM3->SC &= ~(FTM_SC_TOF_MASK); int lSpeed = Motor::lMotorSpeed; if(lSpeed >= 0) { FTM3->CONTROLS[1].CnV = lSpeed; FTM3->CONTROLS[0].CnV = 0; } else { FTM3->CONTROLS[1].CnV = 0; FTM3->CONTROLS[0].CnV = -lSpeed; } int rSpeed = Motor::rMotorSpeed; if(rSpeed >= 0) { FTM3->CONTROLS[4].CnV = rSpeed; FTM3->CONTROLS[5].CnV = 0; } else { FTM3->CONTROLS[4].CnV = 0; FTM3->CONTROLS[5].CnV = -rSpeed; } FTM3->SYNC |= 1 << 7; // trigger SW sync } } void init_ADC16(void){ adc16_config_t config; ADC16_GetDefaultConfig(&config); config.resolution = kADC16_ResolutionSE10Bit; config.enableHighSpeed = true; ADC16_Init(ADC0, &config); ADC16_DoAutoCalibration(ADC0); } volatile unsigned char cameraPulseState = 0; volatile unsigned char generateSI = 0; volatile unsigned int cameraScanTime = 50000; volatile unsigned char currentCameraSwapBuffer = 0; volatile unsigned short cameraData[2][128]; // 2 swap buffers volatile unsigned char isCameraBufferReady[2] = {0}; volatile char isCameraPeriodReady = 0; volatile unsigned char cameraPeriodWriteCount[2]; inline void enablePIT(pit_chnl_t channel, uint32_t count) { PIT_StopTimer(PIT, channel); PIT_DisableInterrupts(PIT, channel, kPIT_TimerInterruptEnable); PIT_SetTimerPeriod(PIT, channel, count); PIT_StartTimer(PIT, channel); PIT_EnableInterrupts(PIT, channel, kPIT_TimerInterruptEnable); EnableIRQ(PIT0_IRQn); EnableIRQ(PIT1_IRQn); EnableIRQ(PIT2_IRQn); EnableIRQ(PIT3_IRQn); } void setupCamera() { init_ADC16(); PORTB->PCR[2] = PORT_PCR_MUX(0); // Camera ADC PORTC->PCR[7] = PORT_PCR_MUX(1); // Camera CLK PORTC->PCR[0] = PORT_PCR_MUX(1); // Camera SI PORTB->PCR[3] = PORT_PCR_MUX(1);// (0) // Camera ADC2 // it will work as switch for stepper detector PORTB->PCR[3] = PORT_PCR_MUX(1) | (1 << 24); // GPIO GPIOB->PDDR &= ~(1 << 3); PORTD->PCR[3] = PORT_PCR_MUX(1); // Camera CLK2 PORTC->PCR[12] = PORT_PCR_MUX(1); // Camera SI2 GPIOC->PCOR |= 1 << 7; GPIOC->PCOR |= 1 << 0; GPIOD->PCOR |= 1 << 3; GPIOC->PCOR |= 1 << 12; GPIOC->PDDR |= 1 << 7; GPIOC->PDDR |= 1 << 0; GPIOD->PDDR |= 1 << 3; GPIOC->PDDR |= 1 << 12; pit_config_t pitConfig; pitConfig.enableRunInDebug = false; PIT_Init(PIT, &pitConfig); enablePIT(kPIT_Chnl_1, cameraScanTime); enablePIT(kPIT_Chnl_0, 600000); // 100x per second } volatile unsigned int time = 0; char antiFlickerSyncType = 0; extern "C" { void PIT_CHANNEL_0_IRQHANDLER(void) { if(PIT->CHANNEL[0].TFLG & PIT_TFLG_TIF_MASK) { PIT->CHANNEL[0].TFLG = PIT_TFLG_TIF_MASK; for(int i=0; i<14; i++) { if(((cameraScanTime+8000)>>i)<600000) { // 26 if(antiFlickerSyncType != i) { antiFlickerSyncType = i; enablePIT(kPIT_Chnl_0, 600000<<i); } break; } } time += 1<<antiFlickerSyncType; isCameraPeriodReady = 1; GPIOC->PSOR |= 1 << 0; // set SI high GPIOC->PSOR |= 1 << 12; // set SI2 high for(int j=8; j!=0; --j) __asm("NOP"); // 4*3 clocks GPIOC->PSOR |= 1 << 7; // set clock high GPIOD->PSOR |= 1 << 3; // set clock2 high for(int j=8; j!=0; --j) __asm("NOP"); // 4*3 clocks GPIOC->PCOR |= 1 << 0; // set SI low GPIOC->PCOR |= 1 << 12; // set SI2 low for(int j=8; j!=0; --j) __asm("NOP"); // 4*3 clocks for(int i=0; i<18; ++i) { GPIOC->PCOR |= 1 << 7; // set clock low GPIOD->PCOR |= 1 << 3; // set clock2 low for(int j=8; j!=0; --j) __asm("NOP"); // 4*3 clocks GPIOC->PSOR |= 1 << 7; // set clock high GPIOD->PSOR |= 1 << 3; // set clock2 high for(int j=8; j!=0; --j) __asm("NOP"); // 4*3 clocks } enablePIT(kPIT_Chnl_1, cameraScanTime); for(int i=0; i<112; ++i) { GPIOC->PCOR |= 1 << 7; // set clock low GPIOD->PCOR |= 1 << 3; // set clock2 low for(int j=8; j!=0; --j) __asm("NOP"); // 4*3 clocks GPIOC->PSOR |= 1 << 7; // set clock high GPIOD->PSOR |= 1 << 3; // set clock2 high for(int j=8; j!=0; --j) __asm("NOP"); // 4*3 clocks } GPIOC->PCOR |= 1 << 7; // set clock low GPIOD->PCOR |= 1 << 3; // set clock2 low PIT->CHANNEL[1].TFLG = PIT_TFLG_TIF_MASK; } } } extern "C" { void PIT_CHANNEL_1_IRQHANDLER(void) { if(PIT->CHANNEL[1].TFLG & PIT_TFLG_TIF_MASK) { PIT->CHANNEL[1].TFLG = PIT_TFLG_TIF_MASK; // clear flag #ifndef CAMERA_AVERAGING enablePIT(kPIT_Chnl_1, 0xFFFFFFFF); #endif GPIOC->PSOR |= 1 << 0; // set SI high GPIOC->PSOR |= 1 << 12; // set SI2 high for(int j=15; j!=0; --j) __asm("NOP"); // 4*3 clocks GPIOC->PSOR |= 1 << 7; // set clock high GPIOD->PSOR |= 1 << 3; // set clock2 high for(int j=15; j!=0; --j) __asm("NOP"); // 4*5 clocks //ADC0_SC1A = AIEN_OFF | DIFF_SINGLE | ADC_SC1_ADCH(6); ADC0->SC1[0] = ADC_SC1_AIEN(0) | ADC_SC1_DIFF(0) | ADC_SC1_ADCH(12); GPIOC->PCOR |= 1 << 0; // set SI low GPIOC->PCOR |= 1 << 12; // set SI2 low for(int j=15; j!=0; --j) __asm("NOP"); // 4*3 clocks for(int i=0; i<128; ++i) { GPIOC->PCOR |= 1 << 7; // set clock low GPIOD->PCOR |= 1 << 3; // set clock2 low for(int j=15; j!=0; --j) __asm("NOP"); // 4*5 clocks while(!(ADC0->SC1[0] & ADC_SC1_COCO_MASK)); short temp = ADC0->R[0]; //temp -= 60; temp-=170; // 150 if(temp&0x800) { temp = 0; } temp = (temp*3)>>1; if(temp&0x700) { temp = 0xFF; } #ifdef CAMERA_AVERAGING if(cameraPeriodWriteCount[currentCameraSwapBuffer]) { cameraData[currentCameraSwapBuffer][i] += temp; } else { cameraData[currentCameraSwapBuffer][i] = temp; } #else cameraData[currentCameraSwapBuffer][i] = temp; #endif GPIOC->PSOR |= 1 << 7; // set clock high GPIOD->PSOR |= 1 << 3; // set clock2 high for(int j=15; j!=0; --j) __asm("NOP"); // 4*5 clocks ADC0->SC1[0] = ADC_SC1_AIEN(0) | ADC_SC1_DIFF(0) | ADC_SC1_ADCH(12); } isCameraBufferReady[currentCameraSwapBuffer] = 1; GPIOC->PCOR |= 1 << 7; // set clock low GPIOD->PCOR |= 1 << 3; // set clock2 low for(int j=15; j!=0; --j) __asm("NOP"); // 4*5 clocks GPIOC->PSOR |= 1 << 7; // set clock high GPIOD->PSOR |= 1 << 3; // set clock2 high for(int j=15; j!=0; --j) __asm("NOP"); // 4*5 clocks GPIOC->PCOR |= 1 << 7; // set clock low GPIOD->PCOR |= 1 << 3; // set clock2 low #ifdef CAMERA_AVERAGING cameraPeriodWriteCount[currentCameraSwapBuffer]++; #endif } } } // pointer obtained by getNextImage is valid till next call of getNextImage unsigned short* getNextImage() { // will return null if there is no image yet unsigned char oldBuffer = currentCameraSwapBuffer; if(isCameraBufferReady[oldBuffer] && isCameraPeriodReady) { // we need to change current buffer without changing current buffer // to value which is bigger than bufferCount - 1 currentCameraSwapBuffer ^= 1; isCameraBufferReady[oldBuffer] = 0; unsigned short* oldData = (unsigned short*)cameraData[oldBuffer]; #ifdef CAMERA_AVERAGING unsigned char writeCount = cameraPeriodWriteCount[oldBuffer]; for(int i = 0; i<128; i++) { oldData[i] = oldData[i]/writeCount; } cameraPeriodWriteCount[oldBuffer] = 0; #endif isCameraPeriodReady = 0; return oldData; } return 0; } // info about camera values, updated at call of calibrateCamera unsigned char cameraMinValue = 0; unsigned char cameraMaxValue = 0; unsigned char cameraMiddleValue = 0; void calibrateCamera(unsigned short* cameraData) { unsigned int minVal = 1020; unsigned int maxVal = 0; for(int j=125; j; j--) { unsigned short average = ((unsigned short)cameraData[j+0]) + ((unsigned short)cameraData[j+1]) + ((unsigned short)cameraData[j+2]) + ((unsigned short)cameraData[j+3]); if(average > maxVal ) { maxVal = average; } else if(average < minVal ) { minVal = average; } } if(minVal == 0 && maxVal == 1020) { return; } int middle = minVal + maxVal; if(middle < 8) middle = 8; cameraScanTime *= 1024.0f / (float)middle; } void cameraInitialCalibration() { for(int i=0; i<40;) { unsigned short* cameraData = getNextImage(); if(cameraData) { i++; calibrateCamera(cameraData); } } } void initI2C() { i2c_master_config_t config = { .enableMaster = true, .enableStopHold = false, .baudRate_Bps = 400000, .glitchFilterWidth = 0 }; I2C_MasterInit(I2C0, &config, 12000000U); PORTE->PCR[24] = PORT_PCR_MUX(5); PORTE->PCR[25] = PORT_PCR_MUX(5); } void setupIOExpander() { uint8_t data[] = { 6, 0b11111111, 0b10111111 }; // 1 = input, 0 = output i2c_master_transfer_t transfer; transfer.flags = kI2C_TransferDefaultFlag; transfer.slaveAddress = 0x24; transfer.direction = kI2C_Write; transfer.subaddress = 0; transfer.subaddressSize = 0; transfer.data = data; transfer.dataSize = sizeof(data); I2C_MasterTransferBlocking(I2C0, &transfer); PORTC->PCR[1] = PORT_PCR_MUX(1) | (1 << 24) | (0b1010 << 16); // GPIO + interrupt + falling edge interrupt type GPIOC->PDDR &= ~(1 << 1); } struct ExpanderData { bool sw1 : 1; bool sw2 : 1; bool sw3 : 1; bool sw4 : 1; bool btn1 : 1; bool btn2 : 1; bool btn3 : 1; bool btn4 : 1; bool btn5 : 1; bool btKey : 1; bool btState : 1; bool undef1 : 1; bool undef2 : 1; bool undef3 : 1; bool undef4 : 1; bool undef5 : 1; }; ExpanderData readIOExapnder() { ExpanderData data; i2c_master_transfer_t transfer; transfer.flags = kI2C_TransferDefaultFlag; transfer.slaveAddress = 0x24; transfer.direction = kI2C_Read; transfer.subaddress = 0; transfer.subaddressSize = 1; transfer.data = (uint8_t*)&data; transfer.dataSize = 2; I2C_MasterTransferBlocking(I2C0, &transfer); return data; } void setupGyroscope() { uint8_t data[] = { 0x13, 0b00000010 }; // CTRL_REG1[ACTIVE] = 1 i2c_master_transfer_t transfer; transfer.flags = kI2C_TransferDefaultFlag; transfer.slaveAddress = 0x20; transfer.direction = kI2C_Write; transfer.subaddress = 0; transfer.subaddressSize = 0; transfer.data = data; transfer.dataSize = sizeof(data); I2C_MasterTransferBlocking(I2C0, &transfer); PORTB->PCR[20] = PORT_PCR_MUX(1) | (1 << 24) | (0b1010 << 16); // INT1 // GPIO + interrupt + falling edge interrupt type GPIOB->PDDR &= ~(1 << 20); PORTE->PCR[26] = PORT_PCR_MUX(1) | (1 << 24) | (0b1010 << 16); // INT2 // GPIO + interrupt + falling edge interrupt type GPIOE->PDDR &= ~(1 << 26); } void setupAccelerometerAndMagnetometer() { const int commandLen = 2; const int commandCount = 5; uint8_t data[commandCount][commandLen] = { { 0x2A, 0b00000000 }, // set to standy mode { 0x5B, 0b00011111 }, // setup magnetometer { 0x5C, 0b00100000 }, // setup magnetometer part 2 { 0x0E, 0b00000001 }, // set accelerometer range { 0x2A, 0b00001101 } // launch accelerometer }; i2c_master_transfer_t transfer; transfer.flags = kI2C_TransferDefaultFlag; transfer.slaveAddress = 0x20; transfer.direction = kI2C_Write; transfer.subaddress = 0; transfer.subaddressSize = 0; for(int i=0; i<commandCount; i++) { transfer.data = data[i]; transfer.dataSize = commandLen; I2C_MasterTransferBlocking(I2C0, &transfer); } { const int commandLen = 2; const int commandCount = 1; uint8_t data[commandCount][commandLen] = { { 0x00, 0b00000000 } }; i2c_master_transfer_t transfer; transfer.flags = kI2C_TransferDefaultFlag; transfer.slaveAddress = 0x20; transfer.direction = kI2C_Read; transfer.subaddress = 0; transfer.subaddressSize = 0; for(int i=0; i<commandCount; i++) { transfer.data = data[i]; transfer.dataSize = commandLen; I2C_MasterTransferBlocking(I2C0, &transfer); } } } namespace Bluetooth { void setup() { uart_config_t uartConfig; uartConfig.baudRate_Bps = 9600; uartConfig.parityMode = kUART_ParityDisabled; uartConfig.stopBitCount = kUART_OneStopBit; uartConfig.txFifoWatermark = 0; uartConfig.rxFifoWatermark = 1; uartConfig.enableRxRTS = false; uartConfig.enableTxCTS = false; uartConfig.idleType = kUART_IdleTypeStartBit; uartConfig.enableTx = true; uartConfig.enableRx = true; UART_Init(UART3, &uartConfig, 60000000); PORTC->PCR[16] = PORT_PCR_MUX(3); PORTC->PCR[17] = PORT_PCR_MUX(3); } void print(char* text) { while (*text) { while (!(UART3->S1 & UART_S1_TDRE_MASK)); UART3->D = *(text++); } while (!(UART3->S1 & UART_S1_TDRE_MASK)); } void print(char ch) { while (!(UART3->S1 & UART_S1_TDRE_MASK)); UART3->D = ch; while (!(UART3->S1 & UART_S1_TDRE_MASK)); } void print(int i) { char text[16]; itoa(i, text, 10); print(text); } void printLn(char* text) { print(text); print('\n'); } }; namespace Stepper { uint8_t state = 0; int position = 0; int desiredPosition = 0; const uint8_t positionsB[] = { 0b01, 0b11, 0b10, 0b10, 0b00, 0b00, 0b00, 0b01 }; const uint8_t positionsC[] = { 0b00, 0b00, 0b00, 0b10, 0b10, 0b11, 0b01, 0b01 }; char getEndState() { return (GPIOB->PDIR & (1 << 3)) >> 3; } void unsafeStepUp() { state++; state &= 0x07; GPIOB->PSOR = positionsB[state] << 10; GPIOB->PCOR = ((~positionsB[state])&3) << 10; GPIOC->PSOR = positionsC[state] << 10; GPIOC->PCOR = ((~positionsC[state])&3) << 10; position++; } void stepUp() { if(position < 1000) { unsafeStepUp(); } } void unsafeStepDown() { state--; state &= 0x07; GPIOB->PSOR = positionsB[state] << 10; GPIOB->PCOR = ((~positionsB[state])&3) << 10; GPIOC->PSOR = positionsC[state] << 10; GPIOC->PCOR = ((~positionsC[state])&3) << 10; position--; } void stepDown() { if(position > 0) { unsafeStepDown(); } } void calibrate() { while(!getEndState()) { unsafeStepDown(); for(int i=0; i<10000; i++) { __asm("NOP"); } } position = 0; } void setup() { PORTB->PCR[10] = PORT_PCR_MUX(1); PORTB->PCR[11] = PORT_PCR_MUX(1); PORTC->PCR[11] = PORT_PCR_MUX(1); PORTC->PCR[10] = PORT_PCR_MUX(1); GPIOB->PSOR |= 1 << 10; GPIOB->PCOR |= 1 << 11; GPIOC->PCOR |= 1 << 11; GPIOC->PCOR |= 1 << 10; GPIOB->PDDR |= 1 << 10; GPIOB->PDDR |= 1 << 11; GPIOC->PDDR |= 1 << 11; GPIOC->PDDR |= 1 << 10; calibrate(); enablePIT(kPIT_Chnl_2, 600000); } }; extern "C" { void PIT_CHANNEL_2_IRQHANDLER(void) { if(PIT->CHANNEL[2].TFLG & PIT_TFLG_TIF_MASK) { PIT->CHANNEL[2].TFLG = PIT_TFLG_TIF_MASK; if(Stepper::desiredPosition != Stepper::position) { if(Stepper::desiredPosition<Stepper::position) { Stepper::stepDown(); } else { Stepper::stepUp(); } } } } } extern void onEncoderUpdate(); namespace Encoder { float currentSpeed = 0.0f; float readsPerSecond = 20.0f; void setup() { ftm_config_t ftmInfo; FTM_GetDefaultConfig(&ftmInfo); FTM_Init(FTM2, &ftmInfo); FTM_SetQuadDecoderModuloValue(FTM2, 0, -1); ftm_phase_params_t phaseOptions; phaseOptions.phasePolarity = kFTM_QuadPhaseNormal; phaseOptions.enablePhaseFilter = false; phaseOptions.phaseFilterVal = 0; FTM_SetupQuadDecode(FTM2, &phaseOptions, &phaseOptions, kFTM_QuadPhaseEncode); PORTB->PCR[18] = PORT_PCR_MUX(6); // ENC1A PORTB->PCR[19] = PORT_PCR_MUX(6); // ENC1B PORTA->PCR[2] = PORT_PCR_MUX(1); // ENC2A PORTB->PCR[23] = PORT_PCR_MUX(1); // ENC2B enablePIT(kPIT_Chnl_3, 60000000/readsPerSecond); } int read() { int encoderCount = (int16_t)FTM_GetQuadDecoderCounterValue(FTM2); /* Clear counter */ FTM_ClearQuadDecoderCounterValue(FTM2); /* Read direction */ /*if (!(FTM_GetQuadDecoderFlags(FTM2) & kFTM_QuadDecoderCountingIncreaseFlag)) { encoderCount = -encoderCount; }*/ return encoderCount; } }; extern "C" { void PIT_CHANNEL_3_IRQHANDLER(void) { if(PIT->CHANNEL[3].TFLG & PIT_TFLG_TIF_MASK) { PIT->CHANNEL[3].TFLG = PIT_TFLG_TIF_MASK; Encoder::currentSpeed = Encoder::read()*Encoder::readsPerSecond; onEncoderUpdate(); } } } #include "u8g2.h" namespace Display { u8g2_t u8g2; uint8_t u8x8_gpio_and_delay(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int, void *arg_ptr) { int waitClocks; switch(msg) { case U8X8_MSG_GPIO_AND_DELAY_INIT: // called once during init phase of u8g2/u8x8 PORTC->PCR[5] = PORT_PCR_MUX(2); // SCK PORTA->PCR[1] = PORT_PCR_MUX(1); // RES PORTB->PCR[9] = PORT_PCR_MUX(1); // CS PORTC->PCR[4] = PORT_PCR_MUX(1); // DC PORTD->PCR[2] = PORT_PCR_MUX(2); // SDA GPIOA->PDDR |= 1 << 1; // RES GPIOB->PDDR |= 1 << 9; // CS GPIOC->PDDR |= 1 << 4; // DC break; // can be used to setup pins case U8X8_MSG_DELAY_NANO: // delay arg_int * 1 nano second waitClocks = arg_int*40/1000; for(int i=0; i<waitClocks; i++) __asm("NOP"); break; case U8X8_MSG_DELAY_100NANO: // delay arg_int * 100 nano seconds waitClocks = arg_int*40/10; for(int i=0; i<waitClocks; i++) __asm("NOP"); break; case U8X8_MSG_DELAY_10MICRO: // delay arg_int * 10 micro seconds waitClocks = arg_int*40*10; for(int i=0; i<waitClocks; i++) __asm("NOP"); break; case U8X8_MSG_DELAY_MILLI: // delay arg_int * 1 milli second waitClocks = arg_int*40*1000; for(int i=0; i<waitClocks; i++) __asm("NOP"); break; case U8X8_MSG_DELAY_I2C: // arg_int is the I2C speed in 100KHz, e.g. 4 = 400 KHz break; // arg_int=1: delay by 5us, arg_int = 4: delay by 1.25us case U8X8_MSG_GPIO_D0: // D0 or SPI clock pin: Output level in arg_int //case U8X8_MSG_GPIO_SPI_CLOCK: break; case U8X8_MSG_GPIO_D1: // D1 or SPI data pin: Output level in arg_int //case U8X8_MSG_GPIO_SPI_DATA: break; case U8X8_MSG_GPIO_D2: // D2 pin: Output level in arg_int break; case U8X8_MSG_GPIO_D3: // D3 pin: Output level in arg_int break; case U8X8_MSG_GPIO_D4: // D4 pin: Output level in arg_int break; case U8X8_MSG_GPIO_D5: // D5 pin: Output level in arg_int break; case U8X8_MSG_GPIO_D6: // D6 pin: Output level in arg_int break; case U8X8_MSG_GPIO_D7: // D7 pin: Output level in arg_int break; case U8X8_MSG_GPIO_E: // E/WR pin: Output level in arg_int break; case U8X8_MSG_GPIO_CS: // CS (chip select) pin: Output level in arg_int if(arg_int) { GPIOB->PSOR |= 1 << 9; } else { GPIOB->PCOR |= 1 << 9; } break; case U8X8_MSG_GPIO_DC: // DC (data/cmd, A0, register select) pin: Output level in arg_int if(arg_int) { GPIOC->PSOR |= 1 << 4; } else { GPIOC->PCOR |= 1 << 4; } break; case U8X8_MSG_GPIO_RESET: // Reset pin: Output level in arg_int if(arg_int) { GPIOA->PSOR |= 1 << 1; } else { GPIOA->PCOR |= 1 << 1; } break; case U8X8_MSG_GPIO_CS1: // CS1 (chip select) pin: Output level in arg_int break; case U8X8_MSG_GPIO_CS2: // CS2 (chip select) pin: Output level in arg_int break; case U8X8_MSG_GPIO_I2C_CLOCK: // arg_int=0: Output low at I2C clock pin break; // arg_int=1: Input dir with pullup high for I2C clock pin case U8X8_MSG_GPIO_I2C_DATA: // arg_int=0: Output low at I2C data pin break; // arg_int=1: Input dir with pullup high for I2C data pin case U8X8_MSG_GPIO_MENU_SELECT: u8x8_SetGPIOResult(u8x8, 0); break; case U8X8_MSG_GPIO_MENU_NEXT: u8x8_SetGPIOResult(u8x8, 0); break; case U8X8_MSG_GPIO_MENU_PREV: u8x8_SetGPIOResult(u8x8, 0); break; case U8X8_MSG_GPIO_MENU_HOME: u8x8_SetGPIOResult(u8x8, 0); break; default: u8x8_SetGPIOResult(u8x8, 1); // default return value break; } return 1; } extern "C" uint8_t u8x8_byte_arduino_hw_spi(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int, void *arg_ptr) { switch(msg) { case U8X8_MSG_BYTE_SEND: dspi_transfer_t transferConfig; transferConfig.dataSize = arg_int; transferConfig.txData = (uint8_t *)arg_ptr; transferConfig.rxData = NULL; transferConfig.configFlags = 0; DSPI_MasterTransferBlocking(SPI0, &transferConfig); break; case U8X8_MSG_BYTE_INIT: dspi_master_config_t spiConfig; spiConfig.whichCtar = kDSPI_Ctar0; spiConfig.ctarConfig.baudRate = u8x8->display_info->sck_clock_hz; spiConfig.ctarConfig.bitsPerFrame = 8; if(u8x8->display_info->spi_mode&0x2) { spiConfig.ctarConfig.cpol = static_cast<dspi_clock_polarity_t>(1); } else { spiConfig.ctarConfig.cpol = static_cast<dspi_clock_polarity_t>(0); } if(u8x8->display_info->spi_mode&0x1) { spiConfig.ctarConfig.cpha = static_cast<dspi_clock_phase_t>(1); } else { spiConfig.ctarConfig.cpha = static_cast<dspi_clock_phase_t>(0); } spiConfig.ctarConfig.direction = kDSPI_MsbFirst; spiConfig.ctarConfig.pcsToSckDelayInNanoSec = 10000; spiConfig.ctarConfig.lastSckToPcsDelayInNanoSec = 10000; spiConfig.ctarConfig.betweenTransferDelayInNanoSec = 10000; spiConfig.whichPcs = kDSPI_Pcs0; spiConfig.pcsActiveHighOrLow = kDSPI_PcsActiveLow; spiConfig.enableContinuousSCK = false; spiConfig.enableRxFifoOverWrite = false; spiConfig.enableModifiedTimingFormat = false; spiConfig.samplePoint = kDSPI_SckToSin0Clock; DSPI_MasterInit(SPI0, &spiConfig, 60000000); u8x8_gpio_SetCS(u8x8, u8x8->display_info->chip_disable_level); break; case U8X8_MSG_BYTE_SET_DC: u8x8_gpio_SetDC(u8x8, arg_int); break; case U8X8_MSG_BYTE_START_TRANSFER: u8x8_gpio_SetCS(u8x8, u8x8->display_info->chip_enable_level); u8x8->gpio_and_delay_cb(u8x8, U8X8_MSG_DELAY_NANO, u8x8->display_info->post_chip_enable_wait_ns, NULL); break; case U8X8_MSG_BYTE_END_TRANSFER: u8x8->gpio_and_delay_cb(u8x8, U8X8_MSG_DELAY_NANO, u8x8->display_info->pre_chip_disable_wait_ns, NULL); u8x8_gpio_SetCS(u8x8, u8x8->display_info->chip_disable_level); break; default: return 0; } return 1; } const uint8_t loading_img[] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x80, 0x00, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0x80, 0x00, 0x3c, 0x00, 0x00, 0x70, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0x80, 0x00, 0x7c, 0x00, 0x38, 0xf0, 0x70, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0x80, 0x00, 0xfc, 0x00, 0x78, 0xf0, 0xf8, 0x00, 0x00, 0x00, 0x01, 0xc0, 0x00, 0x00, 0x00, 0x07, 0xc0, 0x01, 0xfc, 0x00, 0xf8, 0xf1, 0xf0, 0x0e, 0x00, 0x00, 0x03, 0xc0, 0x00, 0x00, 0x00, 0x03, 0xc0, 0x03, 0xf8, 0x01, 0xf8, 0xf3, 0xe0, 0x1e, 0x00, 0xf0, 0x07, 0xc0, 0x00, 0x00, 0x00, 0x03, 0xe0, 0x07, 0xc0, 0x03, 0xf8, 0xf7, 0xe0, 0x3e, 0x01, 0xf0, 0x0f, 0xc0, 0x00, 0x00, 0x00, 0x01, 0xe0, 0x0f, 0xc0, 0x03, 0xf8, 0xff, 0x80, 0x3e, 0x03, 0xf0, 0x0f, 0xc0, 0x00, 0x00, 0x00, 0x01, 0xe0, 0x0f, 0x80, 0x0f, 0xf0, 0x7f, 0x80, 0x7f, 0x07, 0xf0, 0x1f, 0xc0, 0x00, 0x00, 0x00, 0x01, 0xe0, 0x0f, 0x00, 0x1f, 0xf0, 0x7f, 0x80, 0x7f, 0x07, 0xc0, 0x1f, 0xc0, 0x00, 0x00, 0x00, 0x01, 0xf0, 0x1f, 0x00, 0x3f, 0xe0, 0x7f, 0x00, 0xff, 0x0f, 0x80, 0x3f, 0xc0, 0x00, 0x00, 0x00, 0x00, 0xf0, 0x3e, 0x00, 0x3f, 0xe0, 0x7f, 0x00, 0xff, 0x0f, 0x00, 0x3f, 0xc0, 0x00, 0x00, 0x00, 0x00, 0xf0, 0x7c, 0x00, 0x7f, 0xe0, 0x7e, 0x01, 0xff, 0x1f, 0x00, 0x7f, 0xc0, 0x00, 0x00, 0x00, 0x00, 0xf0, 0xf8, 0x00, 0xfb, 0xc0, 0x7c, 0x01, 0xef, 0x1e, 0x00, 0xfb, 0xc0, 0x00, 0x00, 0x00, 0x00, 0xf8, 0xf0, 0x01, 0xf3, 0xc0, 0xf8, 0x01, 0xef, 0x1e, 0x01, 0xf3, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x79, 0xf0, 0x03, 0xe3, 0xc1, 0xf8, 0x01, 0xef, 0x3e, 0x01, 0xe3, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x7b, 0xe0, 0x07, 0xcf, 0xc3, 0xfc, 0x03, 0xef, 0x3c, 0x01, 0xe3, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x7b, 0xc0, 0x0f, 0xff, 0x87, 0xfc, 0x03, 0xcf, 0x7c, 0x03, 0xe3, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x7f, 0xc0, 0x1f, 0xff, 0x87, 0xfc, 0x07, 0xcf, 0xf8, 0x03, 0xc3, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x7f, 0x80, 0x3f, 0xff, 0x8f, 0xbc, 0x07, 0x87, 0xf8, 0x07, 0xc3, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x7f, 0x00, 0x7c, 0xff, 0x1f, 0x3c, 0x07, 0x87, 0xf8, 0x07, 0x83, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x3f, 0x00, 0xf8, 0x0f, 0x3e, 0x3c, 0x07, 0x87, 0xf0, 0x0f, 0x87, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x3e, 0x01, 0xf0, 0x0f, 0x3c, 0x3c, 0x0f, 0x87, 0xf0, 0x1f, 0x87, 0x80, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x03, 0xe0, 0x1f, 0x3c, 0x3c, 0x0f, 0x07, 0xf0, 0x3f, 0xff, 0x80, 0x00, 0x00, 0x00, 0x00, 0x1c, 0x03, 0xc0, 0x1f, 0x38, 0x3c, 0x0f, 0x07, 0xe0, 0x7f, 0xff, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x80, 0x0e, 0x00, 0x3c, 0x1f, 0x07, 0xe0, 0xff, 0xff, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x3c, 0x1e, 0x07, 0xe0, 0xf0, 0x7f, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x1e, 0x03, 0xc1, 0xf0, 0x07, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1c, 0x1e, 0x03, 0xc3, 0xe0, 0x07, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0e, 0x01, 0xc7, 0xe0, 0x07, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0xc0, 0x07, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x80, 0x07, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0f, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xf8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xe0, 0x03, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xfc, 0x07, 0x80, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7f, 0xff, 0xff, 0x07, 0xfc, 0x07, 0x07, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0x87, 0xff, 0x0f, 0x0f, 0x80, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x78, 0x1f, 0xc7, 0xff, 0x0f, 0x0f, 0x81, 0xf8, 0x0e, 0x07, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x78, 0x03, 0x8f, 0xff, 0x0f, 0x1f, 0x83, 0xf0, 0x1e, 0x0f, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x78, 0x01, 0x0f, 0x0f, 0x1f, 0x1f, 0x07, 0xe0, 0x1e, 0x3f, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x78, 0x00, 0x0f, 0x1f, 0x1e, 0x3e, 0x0f, 0x80, 0x1e, 0xff, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x78, 0x00, 0x0f, 0x1f, 0x1e, 0x3c, 0x0f, 0x00, 0x1f, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x78, 0x00, 0x0f, 0x3e, 0x1e, 0x3c, 0x1f, 0x00, 0x1f, 0xf8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x78, 0x00, 0x0f, 0x3e, 0x1e, 0x7c, 0x1e, 0x00, 0x1f, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x78, 0x00, 0x0f, 0x3c, 0x1e, 0x78, 0x3e, 0x00, 0x1f, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x78, 0x00, 0x1f, 0x3c, 0x1e, 0x78, 0x7c, 0x00, 0x3f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x78, 0x00, 0x1e, 0x3f, 0x1e, 0xf8, 0x78, 0x00, 0x3f, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x78, 0x00, 0x1e, 0x1f, 0x9e, 0xf0, 0x78, 0x3c, 0x3f, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x78, 0x00, 0x3e, 0x0f, 0xde, 0xf0, 0xf8, 0x7e, 0x3f, 0xc0, 0x00, 0x10, 0x89, 0x94, 0x5e, 0x00, 0xf8, 0x00, 0x3e, 0x07, 0xdf, 0xf0, 0xf3, 0xfc, 0x7f, 0xe0, 0x00, 0x11, 0x55, 0x56, 0x50, 0x00, 0xf0, 0x00, 0x1c, 0x03, 0xdf, 0xe0, 0xff, 0xf8, 0x7b, 0xf0, 0x00, 0x11, 0x55, 0x55, 0x56, 0x00, 0xf0, 0x00, 0x00, 0x00, 0x0f, 0xc0, 0xff, 0xf0, 0x79, 0xfc, 0x00, 0x11, 0x5d, 0x54, 0xd2, 0x00, 0xf0, 0x00, 0x00, 0x00, 0x07, 0xc0, 0x7f, 0xc0, 0x38, 0xfe, 0x00, 0x1c, 0x95, 0x94, 0x5e, 0xa8, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3e, 0x00, 0x00, 0x7e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x70, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, }; void setup() { u8g2_Setup_ssd1306_128x64_noname_f(&u8g2, U8G2_R0, u8x8_byte_arduino_hw_spi, u8x8_gpio_and_delay); u8g2_InitDisplay(&u8g2); u8g2_SetPowerSave(&u8g2, 0); u8g2_ClearBuffer(&u8g2); u8g2_SetFont(&u8g2, u8g2_font_ncenB08_tr); //u8g2_DrawStr(&u8g2, 0, 10, "Hello world!"); u8g2_DrawBitmap(&u8g2, 0, 0, 16, 64, loading_img); u8g2_SendBuffer(&u8g2); } }; void enableClocks() { CLOCK_EnableClock(kCLOCK_PortA); CLOCK_EnableClock(kCLOCK_PortB); CLOCK_EnableClock(kCLOCK_PortC); CLOCK_EnableClock(kCLOCK_PortD); CLOCK_EnableClock(kCLOCK_PortE); } void setupTFC() { enableClocks(); setupServo(0); Motor::setup(); setupCamera(); initI2C(); setupIOExpander(); setupGyroscope(); setupAccelerometerAndMagnetometer(); Display::setup(); Bluetooth::setup(); Stepper::setup(); Encoder::setup(); } #endif /* __TFC_H__ */