#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>= 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>= 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>= 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<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; iPCR[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::desiredPositionPCR[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; iPSOR |= 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(1); } else { spiConfig.ctarConfig.cpol = static_cast(0); } if(u8x8->display_info->spi_mode&0x1) { spiConfig.ctarConfig.cpha = static_cast(1); } else { spiConfig.ctarConfig.cpha = static_cast(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__ */