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__ */