Last active
January 11, 2024 00:52
-
-
Save dj1711572002/3a07b1dd6ef038d8e5f3f46dc94b83c3 to your computer and use it in GitHub Desktop.
M5StickC NRF24AP2E ANT Reciever
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| //Needs | |
| // Auto shift down/up | |
| #include <M5StickC.h> | |
| //===========M5 display========= | |
| int t=0; | |
| int t_1=0; | |
| // position ofTFT the line on screen | |
| int iHrPosY =62; | |
| int iCdcPosY =30; | |
| int xPos = 160; | |
| boolean stringComplete; | |
| String sensorVal; | |
| String sString; | |
| char sBuf[128]={218,218,218, 'c','d', 217, 218, 219}; | |
| char sensorPrintout[16],old_sensorPrintout[16]; | |
| char cText_old[16],cText[16]; | |
| int x,y,old_y, old_x; | |
| int cursor_x=0; | |
| int cursor_y=95; | |
| int Menu_cursor_x=0; | |
| int Menu_cursor_y=0; | |
| int Menu_cursor_y_max=8; | |
| int iSpd,iSpd_old; | |
| int iCdc,iCdc_old,iCdcX; | |
| int iPwr,iPwr_old,iPwrMax; | |
| int iAvePwr[5]; | |
| int iHr,iHr_old,iHrX; | |
| int iTimeCt; | |
| int i,j; | |
| int i002=0; | |
| int iMode=0; | |
| int GearToothOuter; | |
| int GearToothInner; | |
| short sj; | |
| int iExtrapower=0; | |
| //For ANT+ output | |
| byte eventcount = 0; | |
| volatile uint16_t ANT_power = 0; | |
| volatile uint16_t ANT_Acumpower = 0; | |
| volatile uint16_t ANT_Cdc = 0; // Instant Cadence | |
| //volatile uint16_t accucrankperiod = 0; //Accumulated Crank Period | |
| //volatile uint16_t ANT_Torque = 0; | |
| //#include <SoftwareSerial.h> | |
| // From antmessage.h | |
| #define UCHAR unsigned char | |
| #define MESG_TX_SYNC ((UCHAR)0xA4) | |
| #define MESG_SYSTEM_RESET_ID ((UCHAR)0x4A) | |
| #define MESG_NETWORK_KEY_ID ((UCHAR)0x46) | |
| #define MESG_ASSIGN_CHANNEL_ID ((UCHAR)0x42) | |
| #define MESG_CHANNEL_ID_ID ((UCHAR)0x51) | |
| #define MESG_CHANNEL_RADIO_FREQ_ID ((UCHAR)0x45) | |
| #define MESG_CHANNEL_MESG_PERIOD_ID ((UCHAR)0x43) // Set channel period 0x43 | |
| #define MESG_RADIO_TX_POWER_ID ((UCHAR)0x47) // Set Tx Power 0x47 | |
| #define MESG_CHANNEL_SEARCH_TIMEOUT_ID ((UCHAR)0x44) // Set Channel Search Timeout 0x44 | |
| #define MESG_OPEN_CHANNEL_ID ((UCHAR)0x4B) // ID Byte 0x4B | |
| #define MESG_BROADCAST_DATA_ID ((UCHAR)0x4E) | |
| #define MESG_RESPONSE_EVENT_ID ((UCHAR)0x40) | |
| #define MESG_BROADCAST_DATA_ID ((UCHAR)0x4E) | |
| #define MESG_CAPABILITIES_ID ((UCHAR)0x54) | |
| #define DEBUG 1 | |
| // DEFINITION | |
| #define ANT_CHAN_SPD 3 | |
| #define ANT_CHAN_HRM 4 | |
| #define ANT_CHAN_PWR 5 | |
| #define ANT_NET 0 // public network | |
| #define ANT_TIMEOUT 1 // def=12, 12 * 2.5 = 30 seconds | |
| #define ANT_DEVICETYPE_SPD 123 //0=>wildcard bit 7 = 0 pairing requiest bits 6..0 = 120 for HRM | |
| #define ANT_DEVICETYPE_SPD_CDC 121 //0=>wildcard bit 7 = 0 pairing requiest bits 6..0 = 120 for HRM | |
| #define ANT_DEVICETYPE_HRM 120 //0=>wildcard bit 7 = 0 pairing requiest bits 6..0 = 120 for HRM | |
| // 121-speed cadence 7B=123=spd 122-cdc | |
| #define ANT_DEVICETYPE_PWR 11 //0x0B for PWR | |
| #define ANT_FREQ 57 // Garmin radio frequency | |
| #define ANT_PERIOD_HRM 8070 // //8070 - HRM Garmin search period | |
| #define ANT_PERIOD_PWR 8182 //8182 PWR | |
| #define ANT_PERIOD_SPD 8118 //SP | |
| #define ANT_PERIOD_SPD_CDC 8086//24258 //32344 //SP 8086 | |
| #define NETWORKKEY {0xE8, 0xE4, 0x21, 0x3B, 0x55, 0x7A, 0x67, 0xC} | |
| // For Software Serial | |
| #define ANT_TXD 5 | |
| #define ANT_RXD 4 | |
| //#define ANT_BAUD 9600 // Baud for ANT chip | |
| #define ANT_BAUD 9600 //Br1 2 3=all GND then 4800, BR2(pin10)open=>19200, Baud for ANT chip | |
| #define PACKETREADTIMEOUT 5 //100 original | |
| #define MAXPACKETLEN 64 | |
| #define iBarWidth 6 | |
| // it appears that a Garmin cadence sensor uses pretty much the same values as above except it is a deviceType = 121 | |
| // the tranmission period is supposedly 4.05 MHz or period = 8085 | |
| // Global Variables | |
| int rxBufCnt = 0; | |
| int iCountSpeedLocked; | |
| unsigned char rxBuf[MAXPACKETLEN]; | |
| unsigned char oldHeartRate = 0; | |
| unsigned char Netkey[] = NETWORKKEY; | |
| unsigned long iTime0,iTime1,iTime2,iRev0,iRev1,iRev2,iTime_cdc0,iRev_cdc0; | |
| unsigned long iTimeReset, iTimeReset1; | |
| //SoftwareSerial mySrial23(ANT_RXD, ANT_TXD); | |
| //SoftwareSerial Serial1(4, 5); | |
| //Serial1=Serial1(0, 26);// GPIO0=RX GPIO26=TX | |
| byte ANT_event = 0; | |
| uint16_t ANT_INST_power = 0; | |
| //SoftwareSerial mySrial2345(4, 5);//A0 R | |
| long packetCount = 0; | |
| /* | |
| enum { | |
| errDefault, | |
| errPacketSizeExceeded, | |
| errChecksumError, | |
| errMissingSync | |
| }; | |
| */ | |
| unsigned char writeByte(unsigned char out, unsigned char chksum) { | |
| #ifdef DEBUG | |
| Serial.print(out, HEX); | |
| Serial.print(" "); | |
| #endif | |
| Serial1.write(out); | |
| chksum ^= out; | |
| return chksum; | |
| } | |
| void sendPacket(unsigned msgId, unsigned char argCnt, ...) { | |
| va_list arg; | |
| va_start (arg, argCnt); | |
| unsigned char byteOut; | |
| unsigned char chksum = 0; | |
| int cnt = 0; | |
| chksum = writeByte(MESG_TX_SYNC, chksum); // send sync | |
| chksum = writeByte(argCnt, chksum); // send length | |
| chksum = writeByte(msgId, chksum); // send message id | |
| // send data | |
| for (cnt=1; cnt <= argCnt; cnt++) { | |
| byteOut = va_arg(arg, unsigned int); | |
| chksum = writeByte(byteOut, chksum); | |
| } | |
| va_end(arg); | |
| writeByte(chksum,chksum); // send checksum | |
| #ifdef DEBUG | |
| Serial.println(); | |
| #endif | |
| } | |
| void printPacket(unsigned char * packet) { | |
| int cnt = 0; | |
| while (cnt < packet[1]+4) { | |
| Serial.print(packet[cnt++], HEX); | |
| Serial.print (" "); | |
| } | |
| Serial.println(); | |
| } | |
| int readPacketforInit(unsigned char *packet, int packetSize, int readTimeout) { | |
| unsigned char byteIn; | |
| unsigned char chksum = 0; | |
| //Serial1.listen(); | |
| long timeoutExit = millis() + readTimeout; | |
| while (timeoutExit > millis()) | |
| { | |
| if (Serial1.available() > 0) { | |
| byteIn = Serial1.read(); | |
| timeoutExit = millis() + readTimeout; | |
| if ((byteIn == MESG_TX_SYNC) && (rxBufCnt == 0)) { | |
| rxBuf[rxBufCnt++] = byteIn; | |
| chksum = byteIn; | |
| } else if ((rxBufCnt == 0) && (byteIn != MESG_TX_SYNC)) { | |
| // errorHandler(errMissingSync); | |
| return -1; | |
| } else if (rxBufCnt == 1) { | |
| rxBuf[rxBufCnt++] = byteIn; | |
| chksum ^= byteIn; | |
| } else if (rxBufCnt < rxBuf[1]+3) { | |
| rxBuf[rxBufCnt++] = byteIn; | |
| chksum ^= byteIn; | |
| } else { | |
| rxBuf[rxBufCnt++] = byteIn; | |
| if (rxBufCnt > packetSize) { | |
| // errorHandler(errPacketSizeExceeded); | |
| return -1; | |
| } else { | |
| memcpy(packet, &rxBuf, rxBufCnt); | |
| packetCount++; | |
| if (chksum != packet[rxBufCnt-1]) { | |
| // errorHandler(errChecksumError); | |
| rxBufCnt = 0; | |
| return -1; | |
| } else { | |
| rxBufCnt = 0; | |
| return 1; | |
| } | |
| } | |
| } | |
| } | |
| } | |
| return 0; | |
| } | |
| int readPacket(unsigned char *packet, int packetSize, int readTimeout) { | |
| unsigned char byteIn; | |
| unsigned char chksum = 0; | |
| long timeoutExit = millis() + readTimeout; | |
| //delay(1000); | |
| //while (Serial1.available()<=0){delay(1);} | |
| // Serial1.listen(); | |
| while (timeoutExit > millis()) | |
| { | |
| if ((Serial1.available() > 0)and(rxBufCnt<=63)) { | |
| byteIn = Serial1.read(); | |
| // timeoutExit = millis() + readTimeout; | |
| if ((byteIn == MESG_TX_SYNC) && (rxBufCnt == 0)) { | |
| rxBuf[rxBufCnt++] = byteIn; | |
| chksum = byteIn; | |
| } else if ((rxBufCnt == 0) && (byteIn != MESG_TX_SYNC)) { | |
| // errorHandler(errMissingSync); | |
| return -1; | |
| } else if (rxBufCnt == 1) { | |
| rxBuf[rxBufCnt++] = byteIn; // second byte will be size | |
| chksum ^= byteIn; | |
| } else if (rxBufCnt < rxBuf[1]+3) { // read rest of data taking into account sync, size, and checksum that are each 1 byte | |
| rxBuf[rxBufCnt++] = byteIn; | |
| chksum ^= byteIn; | |
| } else { | |
| rxBuf[rxBufCnt++] = byteIn; | |
| if (rxBufCnt > packetSize) { | |
| // errorHandler(errPacketSizeExceeded); | |
| return -1; | |
| } else { | |
| memcpy(packet, &rxBuf, rxBufCnt); // should be a complete packet. copy data to packet variable, check checksum and return | |
| packetCount++; | |
| if (chksum != packet[rxBufCnt-1]) { | |
| // errorHandler(errChecksumError); | |
| rxBufCnt = 0; | |
| return -1; | |
| } else { | |
| rxBufCnt = 0; | |
| return 1; | |
| } | |
| } | |
| } | |
| } | |
| } | |
| return 0; | |
| // mySrial2345.listen(); | |
| } | |
| int checkReturn() { | |
| byte packet[MAXPACKETLEN]; | |
| int packetsRead; | |
| packetsRead = readPacketforInit(packet, MAXPACKETLEN, PACKETREADTIMEOUT); | |
| delay(50); | |
| if (packetsRead > 0) { | |
| Serial.print("RX: "); | |
| printPacket(packet); | |
| } | |
| return packetsRead; | |
| } | |
| int Init_ANT(){ | |
| // Reset | |
| sendPacket(MESG_SYSTEM_RESET_ID, 1, 0); | |
| delay(600); | |
| // Flush read buffer | |
| while (Serial1.available() > 0) { | |
| Serial1.read(); | |
| } | |
| // Assign Channel | |
| // Channel: 0 | |
| // Channel Type: for Receive Channel | |
| // Network Number: 0 for Public Network | |
| Serial.print("INIT "); | |
| sendPacket(MESG_ASSIGN_CHANNEL_ID, 3, ANT_CHAN_SPD, 0, ANT_NET); | |
| checkReturn(); | |
| // Set Channel ID | |
| // Channel Number: 1forHRM | |
| // Device Number LSB: 0 for a slave to match any device | |
| // Device Number MSB: 0 for a slave to match any device | |
| // Device Type: bit 7 0 for pairing request bit 6..0 for device type | |
| // Transmission Type: 0 to match any transmission type | |
| sendPacket(MESG_CHANNEL_ID_ID, 5, ANT_CHAN_SPD, 0, 0, ANT_DEVICETYPE_SPD_CDC, 0); | |
| checkReturn(); | |
| // Set Network Key | |
| // Network Number | |
| // Key | |
| sendPacket(MESG_NETWORK_KEY_ID, 9, ANT_NET, Netkey[0], Netkey[1], Netkey[2], Netkey[3], Netkey[4], Netkey[5], Netkey[6], Netkey[7]); | |
| checkReturn(); | |
| // Set Channel Search Timeout | |
| // Channel | |
| // Timeout: time for timeout in 2.5 sec increments | |
| sendPacket(MESG_CHANNEL_SEARCH_TIMEOUT_ID, 2, ANT_CHAN_SPD, ANT_TIMEOUT); | |
| checkReturn(); | |
| //ANT_send(1+2, MESG_CHANNEL_RADIO_FREQ_ID, CHAN0, FREQ); | |
| // Set Channel RF Frequency | |
| // Channel | |
| // Frequency = 2400 MHz + (FREQ * 1 MHz) (See page 59 of ANT MPaU) 0x39 = 2457 MHz | |
| sendPacket(MESG_CHANNEL_RADIO_FREQ_ID, 2, ANT_CHAN_SPD, ANT_FREQ); | |
| checkReturn(); | |
| // Set Channel Period | |
| sendPacket(MESG_CHANNEL_MESG_PERIOD_ID, 3, ANT_CHAN_SPD, (ANT_PERIOD_SPD_CDC & 0x00FF), ((ANT_PERIOD_SPD_CDC & 0xFF00) >> 8)); | |
| checkReturn(); | |
| //Open Channel | |
| sendPacket(MESG_OPEN_CHANNEL_ID, 1, ANT_CHAN_SPD); | |
| checkReturn(); | |
| //----------------------SPD in Different Channel | |
| /* | |
| sendPacket(MESG_ASSIGN_CHANNEL_ID, 3, 2, 0, ANT_NET); //2=CH | |
| if (checkReturn() == 0) errorHandler(errDefault); | |
| sendPacket(MESG_CHANNEL_ID_ID, 5, 2, 0, 0, ANT_DEVICETYPE_SPD_CDC, 0); | |
| if (checkReturn() == 0) errorHandler(errDefault); | |
| sendPacket(MESG_NETWORK_KEY_ID, 9, ANT_NET, Netkey[0], Netkey[1], Netkey[2], Netkey[3], Netkey[4], Netkey[5], Netkey[6], Netkey[7]); | |
| if (checkReturn() == 0) errorHandler(errDefault); | |
| sendPacket(MESG_CHANNEL_SEARCH_TIMEOUT_ID, 2, 2, ANT_TIMEOUT); | |
| if (checkReturn() == 0) errorHandler(errDefault); | |
| sendPacket(MESG_CHANNEL_RADIO_FREQ_ID, 2, 2, ANT_FREQ); | |
| if (checkReturn() == 0) errorHandler(errDefault); | |
| sendPacket(MESG_CHANNEL_MESG_PERIOD_ID, 3, 2, (ANT_PERIOD_HRM & 0x00FF), ((ANT_PERIOD_HRM & 0xFF00) >> 8)); | |
| if (checkReturn() == 0) errorHandler(errDefault); | |
| sendPacket(MESG_OPEN_CHANNEL_ID, 1, 2); | |
| if (checkReturn() == 0) errorHandler(errDefault); | |
| */ | |
| //---------------------------------------------------------------------------------------------- | |
| //----------------------------------------FOR HRM----------------------------------------------- | |
| //---------------------------------------------------------------------------------------------- | |
| // Assign Channel | |
| // Channel: 0 | |
| // Channel Type: for Receive Channel | |
| // Network Number: 0 for Public Network | |
| sendPacket(MESG_ASSIGN_CHANNEL_ID, 3, ANT_CHAN_HRM, 0, ANT_NET); | |
| checkReturn(); | |
| // Set Channel ID | |
| // Channel Number: 0 | |
| // Device Number LSB: 0 for a slave to match any device | |
| // Device Number MSB: 0 for a slave to match any device | |
| // Device Type: bit 7 0 for pairing request bit 6..0 for device type | |
| // Transmission Type: 0 to match any transmission type | |
| sendPacket(MESG_CHANNEL_ID_ID, 5, ANT_CHAN_HRM, 0, 0, ANT_DEVICETYPE_HRM, 0); | |
| checkReturn(); | |
| // Set Network Key | |
| // Network Number | |
| // Key | |
| sendPacket(MESG_NETWORK_KEY_ID, 9, ANT_NET, Netkey[0], Netkey[1], Netkey[2], Netkey[3], Netkey[4], Netkey[5], Netkey[6], Netkey[7]); | |
| checkReturn(); | |
| // Set Channel Search Timeout | |
| // Channel | |
| // Timeout: time for timeout in 2.5 sec increments | |
| sendPacket(MESG_CHANNEL_SEARCH_TIMEOUT_ID, 2, ANT_CHAN_HRM, ANT_TIMEOUT); | |
| checkReturn(); | |
| //ANT_send(1+2, MESG_CHANNEL_RADIO_FREQ_ID, CHAN0, FREQ); | |
| // Set Channel RF Frequency | |
| // Channel | |
| // Frequency = 2400 MHz + (FREQ * 1 MHz) (See page 59 of ANT MPaU) 0x39 = 2457 MHz | |
| sendPacket(MESG_CHANNEL_RADIO_FREQ_ID, 2, ANT_CHAN_HRM, ANT_FREQ); | |
| checkReturn(); | |
| // Set Channel Period | |
| sendPacket(MESG_CHANNEL_MESG_PERIOD_ID, 3, ANT_CHAN_HRM, (ANT_PERIOD_HRM & 0x00FF), ((ANT_PERIOD_HRM & 0xFF00) >> 8)); | |
| checkReturn(); | |
| //Open Channel | |
| sendPacket(MESG_OPEN_CHANNEL_ID, 1, ANT_CHAN_HRM); | |
| checkReturn(); | |
| //----------------------------------------FOR POWER | |
| sendPacket(MESG_ASSIGN_CHANNEL_ID, 3, ANT_CHAN_PWR, 0, ANT_NET); | |
| checkReturn(); | |
| delay(600); | |
| sendPacket(MESG_CHANNEL_ID_ID, 5, ANT_CHAN_PWR, 0, 0, ANT_DEVICETYPE_PWR, 0); | |
| checkReturn(); | |
| delay(600); | |
| sendPacket(MESG_NETWORK_KEY_ID, 9, ANT_NET, Netkey[0], Netkey[1], Netkey[2], Netkey[3], Netkey[4], Netkey[5], Netkey[6], Netkey[7]); | |
| checkReturn(); | |
| delay(600); | |
| sendPacket(MESG_CHANNEL_SEARCH_TIMEOUT_ID, 2, ANT_CHAN_PWR, ANT_TIMEOUT); | |
| checkReturn(); | |
| delay(600); | |
| sendPacket(MESG_CHANNEL_RADIO_FREQ_ID, 2, ANT_CHAN_PWR, ANT_FREQ); | |
| checkReturn(); | |
| delay(600); | |
| sendPacket(MESG_CHANNEL_MESG_PERIOD_ID, 3, ANT_CHAN_PWR, (ANT_PERIOD_PWR & 0x00FF), ((ANT_PERIOD_PWR & 0xFF00) >> 8)); | |
| checkReturn(); | |
| delay(600); | |
| sendPacket(MESG_OPEN_CHANNEL_ID, 1, ANT_CHAN_PWR); | |
| checkReturn(); | |
| delay(600); | |
| //--------------- | |
| Serial.println("rx Config Done"); | |
| //send power--------------------------------------------------- | |
| //netkey | |
| sendPacket(MESG_NETWORK_KEY_ID, 9, 0x00, 0xB9, 0xA5, 0x21, 0xFB, 0xBD, 0x72, 0xC3, 0x45); | |
| checkReturn(); | |
| delay(600); | |
| //assignCH | |
| sendPacket(MESG_ASSIGN_CHANNEL_ID, 3, 0x00, 0x10, 0x00); | |
| checkReturn(); | |
| delay(600); | |
| // ch id | |
| sendPacket(MESG_CHANNEL_ID_ID, 5, 0x00, 0x08, 0x00, 0x0B, 0x05);//0x08 device ID | |
| checkReturn(); | |
| delay(600); | |
| //freq | |
| sendPacket(MESG_CHANNEL_RADIO_FREQ_ID, 2, 0x00, 0x39); | |
| checkReturn(); | |
| delay(600); | |
| //ch Period | |
| sendPacket(MESG_CHANNEL_MESG_PERIOD_ID, 3, 0x00, 0xF6, 0x1F); | |
| checkReturn(); | |
| delay(600); | |
| //txPower | |
| sendPacket(MESG_RADIO_TX_POWER_ID, 2, 0x00, 0x03); | |
| checkReturn(); | |
| delay(600); | |
| //Timeout | |
| sendPacket(MESG_CHANNEL_SEARCH_TIMEOUT_ID, 2, 0x00, 0x1E); | |
| checkReturn(); | |
| delay(600); | |
| //openCH | |
| sendPacket(MESG_OPEN_CHANNEL_ID, 1, 0x00); | |
| } | |
| UCHAR checkSum(UCHAR *data, int length) | |
| { | |
| int i; | |
| UCHAR chksum = data[0]; | |
| for (i = 1; i < length; i++) | |
| chksum ^= data[i]; // +1 since skip prefix sync code, we already counted it | |
| return chksum; | |
| } | |
| void ANTsend(uint8_t buf[], int length){ | |
| //Serial.print("ANTsend TX: "); | |
| for(int i = 0 ; i <= length ; i++) | |
| { | |
| // Serial.print(buf[i], HEX); | |
| // Serial.print(" "); | |
| Serial1.write(buf[i]); | |
| } | |
| //Serial.println(""); | |
| } | |
| void send_ANTpower() | |
| { | |
| //Serial.print("ins"); | |
| //Serial.println(ANT_INST_power); | |
| uint8_t buf[13]; | |
| buf[0] = 0xA4; | |
| buf[1] = 0x09; | |
| buf[2] = 0x4E; | |
| buf[3] = 0x00; // Channel number | |
| buf[4] = 0x10; // Simple power identifier | |
| buf[5] = ANT_event; | |
| buf[6] = 0x32; // 50% Differential | |
| buf[7] = (iCdc/10); | |
| buf[8] = byte(ANT_Acumpower & 0xFF); // LSB | |
| buf[9] = byte((ANT_Acumpower >> 8) & 0xFF); // MSB | |
| buf[10] = byte(ANT_INST_power & 0xFF);; // LSB | |
| buf[11] = byte((ANT_INST_power >> 8) & 0xFF); // MSB | |
| buf[12] = checkSum(buf, 12); | |
| ANTsend(buf, 13); | |
| ANT_event += 1; | |
| } | |
| int chkRx() { | |
| byte packet[MAXPACKETLEN]; | |
| int packetsRead; | |
| packetsRead = readPacketforInit(packet, MAXPACKETLEN, PACKETREADTIMEOUT); | |
| // Data <sync> <len> <msg id> <channel> <msg id being responded to> <msg code> <chksum> | |
| // <sync> always 0xa4 | |
| // <msg id> always 0x40 denoting a channel response / event | |
| // <msg code? success is 0. See page 84 of ANT MPaU for other codes | |
| if (packetsRead > 0) { | |
| Serial.print("RX: "); | |
| printPacket(packet); | |
| } | |
| return packetsRead; | |
| } | |
| void printHeader(const char * title) { | |
| Serial.print(millis()); | |
| Serial.print(" "); | |
| Serial.print(packetCount); | |
| Serial.print(" - "); | |
| Serial.print(title); | |
| } | |
| //==============================SETUP============================================================== | |
| //================================================================================================== | |
| //================================================================================================== | |
| void setup() { | |
| M5.begin(); | |
| M5.Lcd.fillScreen(BLACK); | |
| M5.Lcd.setRotation(3); | |
| M5.Lcd.setTextSize(2); // 文字サイズを2にする | |
| M5.Lcd.setCursor(10, 10); | |
| M5.Lcd.print("Wait ANTInit"); | |
| //--------------------------------------- | |
| Serial.begin(115200); | |
| Serial1.begin(9600, SERIAL_8N1, 26, 0); // EXT_IO To NRF AP2E 26->TX 0->RX | |
| Serial.println("Starting..."); | |
| //pinMode(ANT_RXD, INPUT); | |
| //pinMode(ANT_TXD, OUTPUT); | |
| //pinMode(2, INPUT_PULLUP); | |
| //Serial1.begin(ANT_BAUD); | |
| // mySrial2345.begin(9600); | |
| // mySrial2345.println("Starting45..."); | |
| // mySerial.begin(9600); | |
| Serial.println("Config Starting"); | |
| // lcd.begin(8, 2); | |
| // Print a message to the LCD. | |
| // lcd.print("Hello!"); | |
| delay(1500); | |
| // lcd.clear(); | |
| // Reset | |
| sendPacket(MESG_SYSTEM_RESET_ID, 1, 0); | |
| delay(600); | |
| // Flush read buffer | |
| while (Serial1.available() > 0) { | |
| Serial1.read(); | |
| } | |
| Init_ANT(); | |
| iTimeReset=millis(); | |
| //setup_watchdog(1); | |
| //setup_nRF24AP2(); | |
| } | |
| //*******************************LOOP******************************************************************************************** | |
| //******************************************************************************************************************************* | |
| //******************************************************************************************************************************* | |
| void loop() { | |
| byte packet[MAXPACKETLEN]; | |
| int packetsRead; | |
| unsigned char msgId, msgSize; | |
| unsigned char *msgData; | |
| unsigned long iSpeed ; | |
| /* | |
| Serial.print("iRrev1="); | |
| Serial.print(iRev1); | |
| Serial.print(","); | |
| Serial.print("iRrev2="); | |
| Serial.print(iRev2); | |
| Serial.print(","); | |
| */ | |
| if (digitalRead(2)==0){iExtrapower=100;}else{iExtrapower=0;}; | |
| if (iMode ==0){ //iMode 0=Normal; | |
| chkserialEvent(); | |
| packetsRead = readPacket(packet, MAXPACKETLEN, PACKETREADTIMEOUT); | |
| if (packetsRead < 1) {iCountSpeedLocked += 1; | |
| // Serial.print("sCPL");Serial.print(iCountSpeedLocked); | |
| if (iCountSpeedLocked == 5000){iCountSpeedLocked = 0; Init_ANT();} ; | |
| }; | |
| if (packetsRead > 0) { | |
| // if (packetsRead == 9990) { | |
| iCountSpeedLocked = 0; | |
| msgId = packet[2]; | |
| msgSize = packet[1]; | |
| msgData = &packet[3]; | |
| switch (msgId) { | |
| case MESG_RESPONSE_EVENT_ID: | |
| break; | |
| case MESG_CAPABILITIES_ID: | |
| printHeader("MESG_CAPABILITIES_ID: "); | |
| printPacket(packet); | |
| break; | |
| case MESG_BROADCAST_DATA_ID: | |
| //------------HRM | |
| if (msgData[1]==0x04 | msgData[1]==0x84) { | |
| if (oldHeartRate != msgData[msgSize-1]) { | |
| oldHeartRate = msgData[msgSize-1]; | |
| // printHeader("New Heart Rate: "); | |
| Serial.print("H:"); | |
| Serial.print(oldHeartRate); | |
| Serial.print(" "); | |
| Serial.println(" "); | |
| //================================================================== | |
| //==========================M5 HRM Display============================== | |
| //M5.Lcd.fillScreen(BLACK); | |
| M5.Lcd.setRotation(3); | |
| M5.Lcd.setTextSize(2); // 文字サイズを2にする | |
| //================================================================== | |
| //================================================================= | |
| iHr_old=iHr; | |
| iHr=oldHeartRate; | |
| int tmpCl=0b1111011111111111; | |
| if (iHr > 175) {tmpCl=0b0000000000011111;}; | |
| //TFTDigitWrite(55,iHrPosY, iHr, 3, 0, 5, tmpCl); | |
| // if (iHrX < iHr){iHrX = iHr;}; | |
| // //TFTscreen.fillRect(00,iHrPosY, 60, 24, 0x000000); | |
| // Bar for HRM | |
| int tmpNew = (iHr -100) / 10; | |
| if (tmpNew > 10) {tmpNew = 10;}; | |
| int tmpOld = (iHr_old -100) / 10; | |
| if (tmpOld > 10) {tmpOld = 10;}; | |
| /* DrawBar(tmpNew, tmpOld, 50, iHrPosY); | |
| if (iHr > iHr_old){iHr_old = iHr;}; | |
| */ | |
| } | |
| break; | |
| } | |
| //-----------HRM end; | |
| //------------Power | |
| if (msgData[1]==0x10 && msgData[8]<128) | |
| { | |
| iRev1 = msgData[8]; //8 7=>instant power | |
| iRev1 = iRev1<<8; | |
| iRev1 = iRev1+msgData[7]; | |
| iTime1 = msgData[6]; //6 5=>cumulative power | |
| iTime1 = iTime1<<8; | |
| iTime1= iTime1+msgData[5]; | |
| { | |
| Serial.print("P:"); | |
| Serial.print(iRev1); | |
| Serial.println(" "); | |
| //================================================================== | |
| //==========================M5 POWER Display============================== | |
| //String pstr=String(iPwr); | |
| t_1=t; | |
| t=millis(); | |
| M5.Lcd.fillScreen(BLACK); | |
| M5.Lcd.setRotation(3); | |
| M5.Lcd.setTextSize(2); // 文字サイズを2にする | |
| M5.Lcd.setCursor(10, 10); | |
| M5.Lcd.printf("%3dW",iRev1); | |
| M5.Lcd.setCursor(70, 10); | |
| M5.Lcd.printf("%3dbpm",oldHeartRate); | |
| M5.Lcd.setCursor(10, 40); | |
| M5.Lcd.printf("%4d msec",t-t_1); | |
| //================================================================== | |
| //================================================================= | |
| iPwr_old=iPwr; | |
| iPwr=iRev1; | |
| iAvePwr[5]=iAvePwr[0]; | |
| iAvePwr[1]=iAvePwr[2]; | |
| iAvePwr[2]=iAvePwr[3]; | |
| iAvePwr[3]=iPwr; | |
| iAvePwr[0]=(iAvePwr[1]+iAvePwr[2]+iAvePwr[3])/3; | |
| //iPwr= +5; if (iPwr > 1800){iPwr=0;}; | |
| // ; iAvePwr[0]=211; | |
| if (iPwrMax<iPwr){iPwrMax=iPwr;}; | |
| /* TFTDigitWrite(150,iHrPosY+10, iAvePwr[0], 3, 0, 2, 0b0000011111100000); | |
| TFTDigitWrite(110,iHrPosY, iPwr, 3, 0, 2, 0xFFFF); | |
| TFTDigitWrite(70,iHrPosY, iPwrMax, 3, 0, 1, 0xFFFF); | |
| */ | |
| } | |
| break; | |
| } | |
| //-----------Power end; | |
| //------Non-HRM Error Msg Skip---- | |
| iRev1 = msgData[8];//*0xFF+msgData[4]; | |
| iRev1 = iRev1<<8; | |
| iRev1 = iRev1+msgData[7]; | |
| iTime1 = msgData[6]; | |
| iTime1 = iTime1<<8; | |
| iTime1= iTime1+msgData[5]; | |
| iRev2=iRev1-iRev0; | |
| iTime2=iTime1-iTime0; | |
| iTime0 = iTime1; | |
| iRev0 = iRev1; | |
| iSpeed=211968*iRev2/iTime2*60*60/10000;//2070*1024=211968 | |
| // distance run/ per 1 hour | |
| // dis. in mm | |
| if (iRev2>0){ | |
| //Serial.print("S:"); Serial.print(iSpeed); Serial.println(" "); | |
| // lcd.setCursor(8,0); | |
| // lcd.print("s"); | |
| // lcd.print(iSpeed); | |
| // lcd.print(" "); | |
| // mySrial2345.print("S:"); mySrial2345.print(iSpeed); mySrial2345.println(" "); | |
| iSpd_old=iSpd; | |
| iSpd=iSpeed; | |
| }; | |
| iRev1 = msgData[4]; | |
| iRev1 = iRev1<<8; | |
| iRev1 = iRev1+msgData[3]; | |
| iTime1 = msgData[2]; | |
| iTime1 = iTime1<<8; | |
| iTime1= iTime1+msgData[1]; | |
| iRev2=iRev1-iRev_cdc0; | |
| iTime2=iTime1-iTime_cdc0; | |
| iTime_cdc0 = iTime1; | |
| iRev_cdc0 = iRev1; | |
| /* ================error happened==================== | |
| if (iRev2>0){ | |
| iSpeed=(iTime2*100)/1024;//*60/1; | |
| iSpeed=(iRev2*10000)/iSpeed*60/100; | |
| Serial.print("C:"); Serial.println(iSpeed); // Serial.print(" "); | |
| // mySrial2345.print("C:"); mySrial2345.println(iSpeed); // mySrial2345.print(" "); | |
| // iCdc_old = iCdc; | |
| // iCdc=iSpeed/2; | |
| // lcd.setCursor(12,0); | |
| // lcd.print("_"); | |
| // lcd.print(iSpeed); | |
| // lcd.print(" "); | |
| iCdc=iSpeed; | |
| // display on TFT | |
| //iSpd | |
| // TFTDigitWrite(57,00, iSpd, 3, 1, 4, 0b1111111111111111); | |
| //cdc | |
| int tmpCl=0b0000011111110000; | |
| if (iCdc > 88) {tmpCl=0b0000000000011111;}; | |
| // //TFTDigitWrite(35,iCdcPosY, iCdc, 3, 0, 3, tmpCl); | |
| // TFTDigitWrite(110,iCdcPosY, iCdc, 3, 0, 3, tmpCl); //write cdc | |
| }; | |
| */ | |
| //------------CDC END---- | |
| Serial.println(""); | |
| //delay(500); | |
| break; | |
| default: | |
| printHeader("MESG_ID_UKNOWN: "); | |
| printPacket(packet); | |
| break; | |
| } //end switch | |
| };//en d readpacket | |
| //----------Get Tooth Number | |
| GetToothNumber(); | |
| //----------Get Tooth Number ends | |
| /* | |
| //-----------HEX Dump------- | |
| Serial.print("=====msgData[]========"); | |
| Serial.print(msgData[0],HEX); | |
| Serial.print(","); | |
| Serial.print(msgData[1],HEX); | |
| Serial.print(","); | |
| Serial.print(msgData[2],HEX); | |
| Serial.print(","); | |
| Serial.print(msgData[3],HEX); | |
| Serial.print(","); | |
| Serial.print(msgData[4],HEX); | |
| Serial.print(","); | |
| Serial.print(msgData[5],HEX); | |
| Serial.print(","); | |
| Serial.print(msgData[6],HEX); | |
| Serial.print(","); | |
| Serial.print(msgData[7],HEX); | |
| Serial.print(","); | |
| Serial.println(msgData[8],HEX); | |
| //-----------HEX Dump End------- | |
| */ | |
| if (iTimeCt > 55){ | |
| iTimeCt=0; | |
| iPwr_old=iPwr; | |
| iPwr += iExtrapower; | |
| ANT_Acumpower += (uint16_t)iPwr; | |
| proc_CrankPassed() ; | |
| iPwr=iPwr_old; | |
| // writeTFT(0,0,0); | |
| //delete | |
| //iHr_old=iHr; | |
| //iHr += 10; if (iHr > 199){iHr = 0;}; | |
| //if (iHr > 170){iHr = 60000;}; | |
| //iPwr_old=iPwr; | |
| //iPwr += 25; if (iPwr > 1100){iPwr=0;}; | |
| //iSpd_old=iSpd; iSpd += 10; if (iSpd > 500){iSpd = 0;}; | |
| //iCdc_old=iCdc; | |
| //iCdc += 5; if (iCdc > 4010){iCdc = 0;}; | |
| if (iCdc == 140){iCdc = 3922;}; | |
| // if (iCdcX < iCdc) {iCdcX = iCdc;}; | |
| // if (iHrX < iHr) {iHrX = iHr;}; | |
| // pwr ; iAvePwr[0]=211; | |
| ; iAvePwr[0]=sj; | |
| if (iPwrMax < iPwr){ iPwrMax = iPwr;}; | |
| if (iPwrMax < iAvePwr[0]){ iPwrMax = iAvePwr[0];}; | |
| //TFTDigitWrite(52,iCdcPosY, iAvePwr[0], 3, 0, 4, 0b1111111111111111); | |
| //TFTDigitWrite(145,90, iPwr, 4, 0, 3, 0xFFFF);//ANTPOWER | |
| Serial.print("iPwr="); | |
| Serial.println(iPwr); | |
| /* | |
| //================================================================== | |
| //==========================M5 POWER Display============================== | |
| //String pstr=String(iPwr); | |
| t_1=t; | |
| t=millis(); | |
| M5.Lcd.setRotation(3); | |
| M5.Lcd.setTextSize(2); // 文字サイズを2にする | |
| M5.Lcd.setCursor(10, 10); | |
| M5.Lcd.printf("%3d W",iPwr); | |
| M5.Lcd.setCursor(10, 40); | |
| M5.Lcd.printf("%4d msec",t-t_1); | |
| //================================================================== | |
| //================================================================= | |
| */ | |
| // TFTDigitWrite(135,96, sj, 3, 0, 4, 0xFFFF); | |
| //TFTDigitWrite(120,80, iPwrMax, 4, 0, 1, 0b0000011000010000); | |
| //iSpd | |
| // //TFTDigitWrite(30,5, iSpd, 3, 1, 2, 0b0000011111100000); | |
| //cdc | |
| // int tmpCl=0b0000011111110000; | |
| // if (iCdc > 88) {tmpCl=0b0000000000011111;}; | |
| // //TFTDigitWrite(35,iCdcPosY, iCdc, 3, 0, 3, tmpCl); | |
| // HRM | |
| /* | |
| tmpCl=0b0000011111110000; | |
| if (iHr > 175) {tmpCl=0b0000000000011111;}; | |
| TFTDigitWrite(35,iHrPosY, iHr, 3, 0, 3, tmpCl); | |
| */ | |
| //`time lapsed | |
| //TFTDigitWrite(150,5, ((millis()/1000)%60), 2, 0, 2, 0b0000011111100000); | |
| //TFTDigitWrite(122,5, (millis()/1000/60), 2, 0, 2, 0b0000011111100000); | |
| //Geartooth | |
| //TFTDigitWrite(155,32, GearToothInner, 3, 1, 2, 0b1100011111111111); | |
| //TFTDigitWrite(155,48, GearToothOuter, 3, 1, 2, 0b1000011111110000); | |
| //Serial.print("G:"); Serial.print(GearToothInner); // mySrial2345.print(" "); | |
| //Serial.print(" "); Serial.println(GearToothOuter); // mySrial23mySrial23mySrial23mySrial23mySrial23mySrial23mySrial23mySrial23mySrial23mySrial23mySrial23mySrial23mySrial23mySrial23mySrial23mySrial23mySrial23mySrial2345.print(" "); | |
| int tmpNew = iSpd / 50; | |
| if (tmpNew > 10) {tmpNew = 10;}; | |
| int tmpOld = iSpd_old / 50; | |
| if (tmpOld > 10) {tmpOld = 10;}; | |
| DrawBar(tmpNew, tmpOld, 30, 0); | |
| if (iSpd > iSpd_old){iSpd_old = iSpd;}; | |
| // Bar for HRM | |
| // HRM | |
| /* | |
| tmpNew = (iHr -100) / 10; | |
| if (tmpNew > 10) {tmpNew = 10;}; | |
| tmpOld = (iHr_old -100) / 10; | |
| if (tmpOld > 10) {tmpOld = 10;}; | |
| DrawBar(tmpNew, tmpOld, 50, iHrPosY); | |
| if (iHr > iHr_old){iHr_old = iHr;}; | |
| */ | |
| //bar cdc | |
| /* TFTscreen.fillRect(48,iCdcPosY, iCdc-48, iBarWidth, 0x7e0);//7e0 | |
| if (iCdc>90){ | |
| TFTscreen.fillRect(90,iCdcPosY, iCdc-90, iBarWidth, 0x7ff);//7ff | |
| } | |
| if (iCdc>120){ | |
| TFTscreen.fillRect(120,iCdcPosY, iCdc-120, iBarWidth, 0x36);//7e | |
| } | |
| */ | |
| tmpNew = (iCdc -30) / 10; | |
| if (tmpNew > 10) {tmpNew = 10;}; | |
| tmpOld = (iCdc_old -30) / 10; | |
| if (tmpOld > 10) {tmpOld = 10;}; | |
| /* DrawBar(tmpNew, tmpOld, 50, iCdcPosY); | |
| if (iCdc > iCdc_old){iCdc_old = iCdc;}; | |
| */ | |
| //BAR iPwr | |
| tmpNew = (iPwr -100) / 50; | |
| if (tmpNew > 10) {tmpNew = 10;}; | |
| tmpOld = (iPwr_old -100) / 50; | |
| if (tmpOld > 10) {tmpOld = 10;}; | |
| /* DrawBar(tmpNew, tmpOld, 0, 88); | |
| if (iPwr > iPwr_old){iPwr_old = iPwr;}; | |
| */ | |
| /* int tmpi = (iPwr / 50) * 7; | |
| int tmpX = 0; | |
| // if (tmpi > 117){tmpi = 117;}; | |
| for (int i=0; i <100; i += 7 ){ | |
| int tmpCl = 0b0000011111100000; | |
| if (i > 45){tmpCl = 0b0000011111111111;}; | |
| if (i > 81){tmpCl = 0b0000000000011111;}; | |
| if (tmpi < i){tmpCl = 0b1000010000010000;}; | |
| tmpX = i - 21; | |
| // TFTscreen.drawChar(tmpX, 72, 0x20, 0x0000, tmpCl,1); | |
| TFTscreen.drawChar(tmpX, 88, 0x20, 0x0000, tmpCl,1); | |
| }; | |
| */ | |
| }; | |
| iTimeCt += 1; | |
| //---------- decrease bar graph | |
| if ((iTimeCt % 10) == 0){ | |
| /* if (iCdcX > iCdc) | |
| {iCdcX -= 5; | |
| TFTscreen.fillRect(iCdcX, iCdcPosY, 160, iBarWidth, 0b1000010000010000);//7ff | |
| }; | |
| if (iHrX > iHr) | |
| {iHrX -= 1; | |
| TFTscreen.fillRect(iHrX-40,iHrPosY, 160, iBarWidth, 0b1000010000010000);//7ff | |
| }; | |
| */ | |
| if (iSpd_old > iSpd) | |
| {iSpd_old -= 3;}; | |
| if (iCdc_old > iCdc) | |
| {iCdc_old -= 30; | |
| if (iCdc_old > 200) | |
| {iCdc_old = 30;}; | |
| }; | |
| if (iHr_old > iHr) | |
| {iHr_old -= 3;}; | |
| if (iPwr < iPwr_old){ | |
| iPwr_old -= 50; | |
| }; | |
| }; | |
| //---------- decrease bar graph ends | |
| //---CHK if should reset | |
| /* iTimeReset1=millis()-iTimeReset; | |
| if (iTimeReset1>1000000) { //10000 10 sec | |
| iTimeReset=millis(); | |
| Init_ANT(); | |
| } | |
| */ | |
| } //ends imode0 | |
| //----iMode0 end | |
| //-----iMode=1--------------------------------------------------- | |
| if (iMode ==1){ //iMode 0=Normal; | |
| chkserialEvent(); | |
| // iMode1Screen(); | |
| } | |
| } | |
| //************************************************************************************************** | |
| //**************************************END LOOP***************************************************** | |
| //*************************************************************************************************** | |
| void proc_CrankPassed() { | |
| String sPower = String(iPwr, DEC) + ",c" + String(iCdc, DEC); | |
| //long //sPower=String(ANT_power, DEC)+",c"+String(ANT_icad, DEC)+",A"+pr_ave+",i"+iAcum+",T,"+String((jAcum/iAcum), DEC)+","+String(millis()/1000)+",z"+iZero+",M,"+String(iMaxTorque, DEC); | |
| //sPower=String(ANT_power, DEC)+",c"+String(ANT_Cdc, DEC)+",A"+pr_ave+"T"+String((jAcum/iAcum), DEC)+","+String(millis()/1000); | |
| //+",z"+iZero+",M,"+String(iMaxTorque, DEC); | |
| Serial.println(sPower); | |
| //--send over ANT | |
| send_ANTpower(); | |
| // Serial1.flush(); | |
| } | |
| //void TFTDigitWrite(int iTDWX,int iTDWY, int iTDWValue, int nDigit, int nFloat, int iTDWSize, int iTDW_clB, int iTDW_clG, int iTDW_clR){ | |
| void TFTDigitWrite(int iTDWX,int iTDWY, int iTDWValue, int nDigit, int nFloat, int iTDWSize, int iTDW_cl){ | |
| //x,y,value,digit,float, textsize, color255.255.255 | |
| // TFTscreen.setTextSize(iTDWSize); | |
| // TFTscreen.stroke(00,255,00); | |
| // TFTscreen.text(sensorPrintout, 140, 05); | |
| int tmpValue; | |
| for (int i=0; i<nDigit; i++){ | |
| int tmpX= iTDWX - i*6*iTDWSize; // write from left to right, from LSB to MSB | |
| tmpValue = iTDWValue / (pow(10, i)); | |
| tmpValue = (tmpValue % 10) + 48; | |
| if ((nFloat > 0) && (i == 0)) { | |
| iTDWY = iTDWY + 7; | |
| // TFTscreen.drawChar(tmpX, iTDWY, tmpValue, iTDW_cl,0x000000,1); | |
| iTDWY = iTDWY - 7; | |
| }else | |
| { | |
| // TFTscreen.drawChar(tmpX, iTDWY, tmpValue, iTDW_cl,0b0000000000000000,iTDWSize); | |
| } | |
| }; | |
| //;ll system_sleep(); | |
| } //end of loop | |
| /* | |
| void writeTFT(int iTextx, int iTexty, char cText){ | |
| // read the sensor and map it to the screen height | |
| int sensor = 0; | |
| int drawHeight = map(sensor,0,1023,0,TFTscreen.height()); | |
| old_sensorPrintout[0]=sensorPrintout[0]; | |
| old_sensorPrintout[1]=sensorPrintout[1]; | |
| old_sensorPrintout[2]=sensorPrintout[2]; | |
| old_sensorPrintout[3]=sensorPrintout[3]; | |
| unsigned char byteIn; | |
| //----------time------- | |
| TFTscreen.setTextSize(2); | |
| sensorVal=String((millis()/1000)%60); | |
| sensorVal.toCharArray(sensorPrintout, 4); | |
| TFTscreen.fillRect(00, 0, 160, 20, 0x00);//x0400-green 7E0 //E7FC-whiteGreen 87F0green white | |
| TFTscreen.fillRect(00, 20, 160, 1, 0x7E0);//7E0 //E7FC-whiteGreen 87F0green white | |
| TFTscreen.stroke(00,255,00); | |
| TFTscreen.text(sensorPrintout, 140, 05); | |
| sensorVal=String(millis()/1000/60); | |
| sensorVal.toCharArray(sensorPrintout, 4); | |
| TFTscreen.text(sensorPrintout, 120, 05); | |
| //--------spd | |
| sensorVal=String(iSpd); | |
| sensorVal.toCharArray(sensorPrintout, 4); | |
| sensorVal=String(iSpd_old); | |
| sensorVal.toCharArray(old_sensorPrintout, 4); | |
| TFTscreen.setTextSize(2); | |
| TFTscreen.stroke(00,128,000); | |
| TFTscreen.text(old_sensorPrintout, 0, 5); | |
| TFTscreen.stroke(200,255,200); | |
| TFTscreen.text(sensorPrintout, 0, 5); | |
| //----cdc-------- | |
| sensorVal=String(iCdc); | |
| sensorVal.toCharArray(sensorPrintout, 4); | |
| if (iCdcX < iCdc){iCdcX = iCdc;}; | |
| TFTscreen.fillRect(48,iCdcPosY, iCdc-48, iBarWidth, 0x7e0);//7e0 | |
| if (iCdc>80){ | |
| TFTscreen.fillRect(80,iCdcPosY, iCdc-80, iBarWidth, 0x7ff);//7ff | |
| } | |
| if (iCdc>120){ | |
| TFTscreen.fillRect(120,iCdcPosY, iCdc-120, iBarWidth, 0x36);//7e | |
| } | |
| TFTscreen.fillRect(130,iCdcPosY, 48, 20, 0x000000); | |
| TFTscreen.stroke(255,255,255); | |
| TFTscreen.text(sensorPrintout, 130, iCdcPosY); | |
| //-----------power | |
| sensorVal=String(iPwr); | |
| sensorVal.toCharArray(sensorPrintout, 4); | |
| TFTscreen.fillRect(55,100, 160, 128, 0x000000); | |
| TFTscreen.setTextSize(2); | |
| TFTscreen.stroke(255,255,255); | |
| TFTscreen.text(sensorPrintout, 80, 100); | |
| sensorVal=String(iAvePwr[0]); | |
| sensorVal.toCharArray(sensorPrintout, 4); | |
| TFTscreen.stroke(0,255,0); | |
| TFTscreen.text(sensorPrintout, 120, 100); | |
| if (iPwrMax<iPwr){iPwrMax=iPwr;}; | |
| sensorVal=String(iPwrMax); | |
| sensorVal.toCharArray(sensorPrintout, 4); | |
| TFTscreen.setTextSize(1); | |
| TFTscreen.stroke(0,255,255); | |
| TFTscreen.text(sensorPrintout, 65, 100); | |
| if (xPos <= 10) { | |
| xPos = 160; | |
| } | |
| else { | |
| xPos -= 5; | |
| } | |
| } | |
| */ | |
| void DrawBar(int iTargetValue, int iTargetValue_Old, int tmpX, int tmpY){ | |
| int tmpOrgX = tmpX; | |
| if (iTargetValue_Old < 0){iTargetValue_Old = 0;}; | |
| for (int i = iTargetValue_Old; i < 11; i ++ ){ | |
| int tmpCl = 0b0000011111100000; | |
| if (i > 4){tmpCl = 0b0000011111111111;}; | |
| if (i > 8){tmpCl = 0b0000000000011111;}; | |
| if (iTargetValue < i){tmpCl = 0b1000010000010000;}; | |
| tmpX = (i * 7) + tmpOrgX; | |
| //TFTscreen.drawChar(tmpX, tmpY, 0x20, 0x0000, tmpCl,1); | |
| } | |
| } | |
| void chkserialEvent() { | |
| while (Serial.available()) { | |
| sBuf[i002] = Serial.read(); | |
| if (sBuf[i002] == 13 | sBuf[i002] == 10) { | |
| stringComplete = true; | |
| i002=0; | |
| sBuf[i002]=10; | |
| cursor_x = -29;; | |
| // cursor_x = -9;; | |
| // cursor_y += 9; | |
| if (cursor_y>120){cursor_y=98;}; | |
| } | |
| /* if (sBuf[i002] == 49) { //49 - 1 | |
| Serial.write('s'); | |
| KeyDown(1); | |
| } | |
| if (sBuf[i002] == 52) { | |
| Serial.write('s'); | |
| KeyDown(-1); | |
| } | |
| if (sBuf[i002] == 50) { | |
| Serial.write('d'); | |
| KeyEnter(); | |
| } | |
| */ | |
| if (sBuf[i002] == 65) { //65 - A | |
| sj = 0; | |
| for (int i = 1; i<=4; i ++){ | |
| sBuf[i002] = Serial.read(); | |
| delay(5); | |
| if (((char) sBuf[ i002]) >= 48 && ((char) sBuf[ i002]) <= 58) {// ifvalid number | |
| sj = sj*10 + (((char) sBuf[ i002])-48); | |
| }; | |
| }; | |
| Serial.print("BTinputA:"); | |
| Serial.print(sj); | |
| Serial.println(" "); | |
| Serial.flush(); | |
| } | |
| Serial.write(sBuf[i002]); | |
| // TFTscreen.drawChar(cursor_x,cursor_y,sBuf[i002],0x000f00,0x000000,1); | |
| //TFTscreen.drawChar(cursor_x,cursor_y,sBuf[i002],0b1100011111111000,0b0000000000000000,5); | |
| cursor_x +=30; | |
| i002 +=1; | |
| } | |
| } | |
| void GetToothNumber() | |
| { | |
| double km2mm ; | |
| unsigned long cdc_div60 ; | |
| unsigned long ratio_WheelRev ; | |
| // iSpd = 401; | |
| //iCdc = 80; | |
| km2mm = double(iSpd * 27.77) / 2030;// + 0x77770000;//2030; | |
| // km2mm= 0x190 * 0x115; | |
| cdc_div60 = (iCdc*100)/60; | |
| ratio_WheelRev = (km2mm*1000)/cdc_div60; | |
| GearToothOuter = (52*100)/ratio_WheelRev; | |
| GearToothInner = (39*100)/ratio_WheelRev; | |
| /* | |
| Serial.print("dbg"); Serial.print(km2mm, 4); Serial.print(" "); | |
| Serial.print("dbg"); Serial.print(cdc_div60); Serial.print(" "); | |
| Serial.print("dbg"); Serial.print(ratio_WheelRev); Serial.print(" "); | |
| Serial.print("50:"); Serial.print(GearTooth50); Serial.print(" "); | |
| Serial.print("34:"); Serial.print(GearTooth34); Serial.print(" "); | |
| delay(300); | |
| */ | |
| } | |
| void KeyDown(short bCounter){ | |
| if (iMode==0){ | |
| int tmpY=Menu_cursor_y*16; | |
| // TFTscreen.drawRect(Menu_cursor_x,tmpY, 160, 15, 0x36);//7e | |
| Menu_cursor_y += bCounter; | |
| if (Menu_cursor_y > Menu_cursor_y_max){Menu_cursor_y=0;}; | |
| tmpY=Menu_cursor_y*16; | |
| //TFTscreen.drawRect(Menu_cursor_x,tmpY, 160, 15, 0x7e0);//7e | |
| }; | |
| if (iMode==1){ | |
| Menu_cursor_y += bCounter; | |
| if (Menu_cursor_y > Menu_cursor_y_max){Menu_cursor_y=0;}; | |
| int tmpY=Menu_cursor_y+48; | |
| //TFTscreen.drawChar(70,54,tmpY,0xffff,0x000000,5); | |
| }; | |
| };//end key down | |
| void KeyEnter() { | |
| if (iMode==0){ | |
| if (Menu_cursor_y==7) {iMode=1; | |
| //TFTscreen.fillRect(0, 0, 160, 120, 0x7e0); | |
| Menu_cursor_y_max=50; | |
| };//7ff | |
| }; | |
| if (iMode==1){ | |
| int tmpY=Menu_cursor_y+48; | |
| if (tmpY==58){sString="";} else | |
| { sString=sString+char(tmpY);};//Serial.write(tmpY); | |
| //TFTscreen.drawChar(70,100,tmpY,0xffff,0x000000,2); | |
| // TFTscreen.write(sString, 5, 120); | |
| if (tmpY=59){sString=sString+"/r/n";Serial.println(sString);};// ;=>write to console | |
| if (Menu_cursor_y==Menu_cursor_y_max) {iMode=0;Menu_cursor_y_max=8;}; | |
| }; | |
| } | |
| /* | |
| 1 | |
| 2 | |
| 3 | |
| 4 | |
| 5 | |
| 6 | |
| 7 | |
| 8 | |
| 9 | |
| 10 | |
| Pwr | |
| 150 | |
| 200 | |
| 250 | |
| 300 | |
| 350 | |
| 400 | |
| 450 | |
| 500 | |
| 550 | |
| 600 | |
| Hrm | |
| 100 | |
| 110 | |
| 120 | |
| 130 | |
| 140 | |
| 150 | |
| 160 | |
| 170 | |
| 180 | |
| 190 | |
| Cdc | |
| 30 | |
| 40 | |
| 50 | |
| 60 | |
| 70 | |
| 80 | |
| 90 | |
| 100 | |
| 110 | |
| 120 | |
| Spd | |
| 5 | |
| 10 | |
| 15 | |
| 20 | |
| 25 | |
| 30 | |
| 35 | |
| 40 | |
| 45 | |
| 50 | |
| */ |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment