- 帖子： 179
- 感谢您收到 0
竞赛作品编码
2020 NXP Cup Innovation Challenge contest -Autonomous Racing Car
- btt
- [btt]
-
帖子作者
- 离线
- 管理员
-
Less
更多
2020-09-04 12:16 #78
由 btt
新帖
This project is the first winner on 2020 NXP Cup Electromaker Innovation Challenge contest.
For this project details,please refer autonomous-racing-car-the-first-place
Code:
Main control code
This file controls how should servos and motors behave based on all inputs
Setup and control of peripherals
This code should supply all functions to control all peripherals from main code and get all informations from all sensors.
For this project details,please refer autonomous-racing-car-the-first-place
Code:
Main control code
This file controls how should servos and motors behave based on all inputs
Code:
/*
* 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;
}
Setup and control of peripherals
This code should supply all functions to control all peripherals from main code and get all informations from all sensors.
Code:
#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__ */