竞赛作品编码

2020 NXP Cup Innovation Challenge contest -Autonomous Racing Car

btt
[btt]
帖子作者
离线
管理员

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

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

请登录或注册一个帐号参加讨论