/* ATTENTION: THIS PROGRAM USES BLUETOOTH SYSTEM CALLS FOR AUTO BT-SETUP & BT-CONNECT THEREFOR THIS PROGRAM REQUIRES NXT-FIRMWARE VERSION 1.07 (FOR NXC/NQC) IN YOUR NXT-BRICK 4WD_RCmodule11 = 1.1 version - Changed File name at CAR-Module into "4WD_CARmod11.rxe" 4WD_RCmodule10 = 1.0 version - BT-Messaging compressed (2x4 Boxes into 2x1 Box) for higher BT Communication speed, causing really smooth car control! ADC/CPA Signalling beeps added. 4WD_RCmodule09 = 0.9 version - Park-Reverse gear handling improved BT-Inbox synchronisation improved (Keep polling until Inbox filled) 4WD_RCmodule08 = 0.8 version - Improved BT-Setup sequence with display messages and "wait for Power-Up" of CAR module 4WD_RCmodule07 = 0.7 version - Auto Bluetooth setup (with trouble history in chronological order) >>> BT-Setup NOT OPERATIONAL Using Standard Firmware version 1.04 >>> File Error! message in NXT during RunTime... (Firmware 1.04) v1.28 UnOfficial Firmware loaded in NXT, still file error during Run Time v1.28 AND "Enhanced Firmware" Chekced in BricxCC Preferences [Compiler NBC/NXC] cause NXT HangUp when SysCall(CommExecuteFunction, Args) is used (Reset with Battery removal required...) v1.28 Compiler Switc -v=128 AND "Enhanced Firmware" Chekced in BricxCC Preferences [Compiler NBC/NXC] again causes Run Time "File error" in NXT (using SysCommExecuteFunction(Args) enhanced calls) v1.05 Std Firmware loaded since v1.28 was causing worse BT Communications with version v0.6 of this RCmodule. Still no positive results... v1.07 UnOfficial firmware version loaded... THIS IS WORKING UNDER STRICT CONDITIONS ONLY! (after hours of experiments and trying combinations) I came to this conclusion. No -v=107 switch, no "enhanced" check in BricxCC Preferences... Using old SysCall(CommExecuteFunction, Args) calls with a selfcreated commandstructure type 4WD_RCmodule06 = 0.6 version - Braking & Reverse + PRND Control 4WD_RCmodule05 = 0.5 version - Bluetooth Communication added + CAR Module created 4WD_RCmodule04 = 0.4 version - Blinking Functions, Gas Pedal Monitoring, Steering Monitoring 4WD_RCmodule03 = 0.3 version - Analog Power & Fuel meters added 4WD_RCmodule02 = 0.2 version - Analog Speed meter 4WD_RCmodule01 = 0.1 version - Initial graphic experiments */ #include "NXCDefs.h" // Bluetooth communication constants #define BT_CONN 1 // Use Bluetooth Channel 1 (Must be connectd to other NXT (=car)) // Display Positioning constants per object #define XposDash 0 // Overall Dashbord Picture #define YposDash 0 // #define XposNoCar 16 // NO CAR FOUND Message window #define YposNoCar 19 // #define XposLeftArrow 34 // Richtingaanwijzer Links #define XposRightArrow 52 // Richtingaanwijzer Rechts #define YposArrow 52 // Richtingaanwijzers #define XposADC1 8 // AutoDistanceControl Indicator 1 #define YposADC 45 // #define XposADC2 78 // AutoDistanceControl Indicator 2 #define XposCPA1 6 // CollisionPreventionAssistant Ind.1 #define YposCPA 45 // #define XposCPA2 76 // CollisionPreventionAssistant Ind.2 #define XposPRND 40 // PRND Ind. Automatic Gearing #define YposPRND 8 // #define XposReFuel 69 // Please Refuel indicator #define YposReFuel 1 // #define XposPower 14 // Power meter (Toerenteller) center #define YposPower 23 // #define XposSpeed 49 // Speedo meter center #define YposSpeed 23 // #define XposFuel 88 // Fuel (Battery) meter center #define YposFuel 22 // // Declare X and Y Coordinate Arrays for Analog Meters on NXT Display // These points are calculated as being the End of the Finger in the meter. // These points are the absolute position related to the center of the meter as positioned in the programm // Replacing the center of the meter will require an offset caculation to these poits #define PowerMeterPoints 20 // Number of prepared coördinates in RPM Meter (See array below) int XcoordPower[] = { 7, 5, 4, 4, 3, 3, 3, 3, 4, 5, 6, 8, 9, 11, 13, 14, 16, 18, 20, 21, 22}; // 20 points int YcoordPower[] = {15, 16, 18, 19, 21, 23, 24, 26, 28, 29, 31, 32, 33, 34, 34, 34, 34, 33, 32, 31, 30}; #define SpeedMeterPoints 33 // Number of prepared coördinates in Speed Meter (See array below) int XcoordSpeed[] = {32, 31, 30, 29, 29, 29, 29, 30, 31, 33, 34, 36, 38, 41, 43, 46, 48, 51, 54, 56, 59, 61, 63, 65, 66, 67, 68, 69, 69, 69, 68, 68, 66, 65}; // 33 x-points int YcoordSpeed[] = {12, 15, 17, 20, 22, 25, 27, 30, 32, 35, 37, 38, 40, 41, 42, 43, 43, 43, 42, 42, 41, 39, 37, 36, 33, 31, 29, 26, 23, 21, 18, 16, 13, 11}; // 33 y-points #define FuelMeterPoints 16 // Number of prepared coördinates in Fuel Meter (See array below) int XcoordFuel[] = {81, 79, 78, 78, 79, 80, 82, 85, 88, 91, 94, 96, 97, 98, 98, 97, 95}; // 16 points int YcoordFuel[] = {15, 17, 20, 23, 26, 28, 30, 32, 32, 32, 30, 28, 26, 23, 20, 17, 15}; // State threshold constants #define SteerEdgeMax 45 // Plus or Minu xx Degrees #define FlashLight 30 // Start Left/Right Flashlights when making a turn of xx Degrees or more #define BattEmpty 6900 // mVolt Minimum Batt Level before failure (Fuel = 0) #define BattCharge 15 // xx% of Full Battery = Light Up "Refuel Indicator" on Batt Level check #define BattFull 8100 // mVolt Maximum Voltage after recharge that Equals Fuel = 100% #define Park 1 // Used for PRND variable settings (0=Off) #define Reverse 2 // #define Neutral 3 // #define Drive 4 // #define BT_TimeOut 300 // Terminate Bluetooth communication after xx mSec of continuous Errors #define BeepPause 750 // Miliseconds silence between ADC/CPA Beeps // Sensor conversion ratio's #define GasTick 11 // One Sensor Tick = xx Percent Power #define SteerTick 2 // One Sensor Tick = xx Degrees Steering #define BattRatio 99 // Calculate 100/(BattFull-BattEmpty) voltage increase 0..100% // Global Variables bool ShowDebugInfo=0; // Set when you want to display extra programstate info during execution in NXT bool NoCar=0; // Set when there is no Bluetooth connection with the Car bool NoCar_OLD=0; // NoCar Status during previous display cycle bool SearchCar=0; // Set when Bluetooth connection Setup is in progress bool GoLeft=0; // Set Left BlinkingLight when true bool GoRight=0; // Set Right BlinkingLight when true bool B_GoLeft=0; // Blinking for GoLeft bool B_GoRight=0; // Blinking for GoRight bool ADC=0; // Automatic Distance Control is ACTIVE when true bool CPA=0; // Collision Prevention Assistant is ACTIVE when true short PRND=0; // PRND Gearing Indicator 0=Off 1=P 2=R 3=N 4=D bool Brake=0; // True when brakepedal is pressed for braking while riding bool BrakePressed=0; // Flag for pressing/releasing BrakeSwitch bool ReFuel=0; // Battery is almost Empty (Show indicator) bool B_ReFuel=0; // Blinking for Refuel short MotPwr=0; // Powerlevel 0..100% to Driving Motors short PwrPedal=0; // Gas Pedal position measured with Rotation Sensor short PwrCorrection=0; // Correctionvalue when Gas Pedal is pushed too far short PwrCorrection_OLD=0; // Chached for comparisson in next measurement short Speed=0; // Speedlevel 0..100% measured from short Fuel=0; // Fuel level measured from Battery (Converted to 0..100%) short SteerWheel=0; // Steering Wheel position read from the RC Unit Sensor (in degrees) short SteerCorrection=0; // Correctionvalue when driver is turning steering wheel too far short SteerCorrection_OLD=0; // Chached for comparisson in next measurement short SteerEdge=0; // Steering edge value for frontwheels (in degrees) bool Blink=0; // False = dont show, True= show indicator long B_Time=0; // Measured System Timer for Blinking functions long Disp_Time=0; // Measures times for Diplay messages short Tmp=0; // Scratch Variable long DebugVal=0; // Scratch Variable shown on Displa (Left) when ShowDebugInfo = True // ============================================================================= sub ShowDashboard () // Display All meters & indicators in dashboard { byte FingerPoint=0; // For calculating meterfinger coordinates if (NoCar) { Disp_Time=CurrentTick(); NoCar_OLD=true; } else if (NoCar_OLD) if (CurrentTick()>Disp_Time+100) NoCar_OLD=0; GraphicOut(0, 0, "My_DashBasic.ric", false); // Init screen (clearscreen = false because basic ric covers total screen) if ((NoCar) || (NoCar_OLD) || (SearchCar)) { // ALWAYS ON TOP - MUST BE DIPLAYED AS LAST ITEM if (SearchCar) GraphicOut(XposNoCar, YposNoCar,"My_DashSCar.ric", false); // Setting up BT Connection... else GraphicOut(XposNoCar, YposNoCar,"My_DashNCF.ric", false); // No Car Found warning } else { switch (PRND) { case 0 : GraphicOut(XposPRND, YposPRND, "My_DashPRND_Off.ric", false); // Show Automatic Gearing state break; case 1 : GraphicOut(XposPRND, YposPRND, "My_DashPRND_P.ric", false); break; case 2 : GraphicOut(XposPRND, YposPRND, "My_DashPRND_R.ric", false); break; case 3 : GraphicOut(XposPRND, YposPRND, "My_DashPRND_N.ric", false); break; case 4 : GraphicOut(XposPRND, YposPRND, "My_DashPRND_D.ric", false); break; default : GraphicOut(XposPRND, YposPRND, "My_DashPRND_Off.ric", false); } if (CPA) { GraphicOut(XposCPA1, YposCPA, "My_DashCPA1.ric", false); // Collision Prevention Assistance GraphicOut(XposCPA2, YposCPA, "My_DashCPA1.ric", false); } else if (ADC) { GraphicOut(XposADC1, YposADC, "My_DashADC1.ric", false); // Automatic Distance Control GraphicOut(XposADC2, YposADC, "My_DashADC1.ric", false); } if (B_ReFuel) GraphicOut(XposReFuel, YposReFuel,"My_DashReFuel.ric", false); // ReFuel sign if (B_GoLeft) GraphicOut(XposLeftArrow, YposArrow, "My_DashLAopen1.ric", false); // Go Left sign if (B_GoRight) GraphicOut(XposRightArrow, YposArrow, "My_DashRAopen1.ric", false); // Go Right sign // Display ANALOG METERS FingerPoint = Speed * SpeedMeterPoints / 100; if (FingerPoint>SpeedMeterPoints) FingerPoint=SpeedMeterPoints; // Protect against > 100 value LineOut(XposSpeed, YposSpeed, XcoordSpeed[FingerPoint], YcoordSpeed[FingerPoint], false); LineOut(XposSpeed, YposSpeed+1, XcoordSpeed[FingerPoint], YcoordSpeed[FingerPoint], false); LineOut(XposSpeed+1, YposSpeed, XcoordSpeed[FingerPoint], YcoordSpeed[FingerPoint], false); LineOut(XposSpeed+1, YposSpeed+1, XcoordSpeed[FingerPoint], YcoordSpeed[FingerPoint], false); FingerPoint = MotPwr * PowerMeterPoints / 100; if (FingerPoint>PowerMeterPoints) FingerPoint=PowerMeterPoints; // Protect against > 100 value LineOut(XposPower-1, YposPower, XcoordPower[FingerPoint], YcoordPower[FingerPoint], false); LineOut(XposPower, YposPower-1, XcoordPower[FingerPoint], YcoordPower[FingerPoint], false); LineOut(XposPower+1, YposPower, XcoordPower[FingerPoint], YcoordPower[FingerPoint], false); LineOut(XposPower, YposPower+1, XcoordPower[FingerPoint], YcoordPower[FingerPoint], false); FingerPoint = Fuel * FuelMeterPoints / 100; if (FingerPoint>FuelMeterPoints) FingerPoint=FuelMeterPoints; // Protect against > 100 value LineOut(XposFuel-1, YposFuel, XcoordFuel[FingerPoint], YcoordFuel[FingerPoint], false); LineOut(XposFuel, YposFuel-1, XcoordFuel[FingerPoint], YcoordFuel[FingerPoint], false); LineOut(XposFuel+1, YposFuel, XcoordFuel[FingerPoint], YcoordFuel[FingerPoint], false); LineOut(XposFuel, YposFuel+1, XcoordFuel[FingerPoint], YcoordFuel[FingerPoint], false); } // End of If (NoCar) Else body if (ShowDebugInfo) NumOut(1,55,DebugVal,false); } sub WaitBT_Ready() // Wait for Bluetooth communication in Mailbox is ready { long BT_Tick; // TimeStamp for Bluetooth communications TimeOuts BT_Tick=CurrentTick(); while ((BluetoothStatus(BT_CONN)!=NO_ERR) && (CurrentTick()NO_ERR Status means trouble. // Suggestion: Insert routine to repare BT_Connection while driving here } sub SendToCar() // Send monitored values to Car { long TxValue=0; // This TxSequence using 4 mailboxes worked fine but it can be done faster. // // WaitBT_Ready(); // SendRemoteNumber(BT_CONN,1,MotPwr); // WaitBT_Ready(); // SendRemoteNumber(BT_CONN,2,SteerEdge); // WaitBT_Ready(); // SendRemoteBool(BT_CONN,3,Brake); // WaitBT_Ready(); // SendRemoteNumber(BT_CONN,4,PRND); // Communicating 4 parameters in 1 mailbox at the same time // Binary Format: 0000 0000 0000 0000 0000 // | || | | |_______|__ 8 bits for MotPwr 0..100 // | || |_______|____________ 8 bits for SteerEdge -XX..0..XX (SteerEdgeMax) // | ||______________________ 1 bit for Brake Flag // |_|_______________________ 3 bits for PRND 0..4 (with extra bit for future extensions) // TxValue=PRND; TxValue<<=1; // Add PRND Flags and shift Left 1 Bits TxValue+=Brake; TxValue<<=8; // Add Brake Flag and Shift Left 8 Bits further TxValue+=SteerEdge & 0xFF; TxValue<<=8; // Add Masked SteerEdge and Shift Left 8 Bits further TxValue+=MotPwr; // Add Motor Power level WaitBT_Ready(); SendRemoteNumber(BT_CONN,1,TxValue); } sub ReceiveFromCar() // Receive driving information from car (Master polls Slave, slave has to prepare Tx Buffer) { short BT_Nr_Input; bool BT_Bo_Input; byte BT_State; long RxValue=0; if (BluetoothStatus(BT_CONN)==NO_ERR) { // This first way of InBox reading is fast, but when reading the 3rd and 4th box // their returned BT_State is 0x40, meaning "empty". So the polling is going // faster than the slave can fill his TxBoxes... // I expected to have a good reading during the next loop (calling this sub again later) // but somehow the reading of the 3rd and 4th box stay "empty". // When changing the order of code lines below, the problem shifts to the InBox // number that is polled as 3rd and 4th... So it is some kind of a timing problem... // // BT_State=ReceiveRemoteNumber(1, true, BT_Nr_Input); // if (BT_State==NO_ERR) {Speed=BT_Nr_Input;} // Only accept new info when mailbox was NOT empty. // BT_State=ReceiveRemoteNumber(2, true, BT_Nr_Input); // if (BT_State==NO_ERR) {Fuel=BT_Nr_Input;} // Only accept new info when mailbox was NOT empty. // BT_State=ReceiveRemoteBool( 3, true, BT_Bo_Input); // if (BT_State==NO_ERR) {ADC=BT_Bo_Input;} // Only accept new info when mailbox was NOT empty. // BT_State=ReceiveRemoteBool( 4, true, BT_Bo_Input); // if (BT_State==NO_ERR) {CPA=BT_Bo_Input;} // Only accept new info when mailbox was NOT empty. // I decided to introduce a better Synchronisation method, simply wait for the slave to come... // This works good but causes far more communication delay while waiting for inbox to be filled from slave unit // This delay caused the TX (from RC-2-Car) to be delayed also. // Disadvantage: Slow readings of Speed Meter AND an Interrupted feeling when controlling the Car // turning the steering wheel slowly for example causes the frontwheels to react step-by-step (Car does not respond smoothly) // This method worked 100% correct, but causes slower communications. // // while (NO_ERR!=ReceiveRemoteNumber(1, true, BT_Nr_Input));{} // Keep on polling CAR (=Slave) until message comes // Speed=BT_Nr_Input; // Only accept new info when mailbox was NOT empty. // while (NO_ERR!=ReceiveRemoteNumber(2, true, BT_Nr_Input));{} // Fuel=BT_Nr_Input; // Only accept new info when mailbox was NOT empty. // while (NO_ERR!=ReceiveRemoteBool (3, true, BT_Bo_Input));{} // ADC=BT_Bo_Input; // Only accept new info when mailbox was NOT empty. // while (NO_ERR!=ReceiveRemoteBool (4, true, BT_Bo_Input));{} // CPA=BT_Bo_Input; // Only accept new info when mailbox was NOT empty. // Next attempt, an old binary programming trick... // Prepare RxValue to receive 4 parameters as 1 number in 1 InBox (to reduce Bluetooth communication delays) // This appeared to be a great idea! I also did it with the mailboxes from RC to CAR (See sub SendToCar() above). // This alows very smooth CAR handling! // // Binary Format: 00 0000 0000 0000 0000 // || | | |_______|__ 8 bits for Speed 0..100 // || |_______|____________ 8 bits for Fuel 0..100 // ||______________________ 1 bit for ADC Flag // |_______________________ 1 bit for CPA Flag // BT_State=ReceiveRemoteNumber(1, true, RxValue); if (BT_State==NO_ERR) { // Only accept new info when mailbox was NOT empty. Speed=RxValue & 0xFF; // Mask least significant 8 bits and copy them into Speed RxValue>>=8; // Shift 8 bits right to select next reading Fuel=RxValue & 0xFF; // Mask least significant 8 bits and copy them into Speed RxValue>>=8; ADC=RxValue & 0x01; // Mask least significant 8 bits and copy them into Speed RxValue>>=1; CPA=RxValue & 0x01; // Mask least significant 8 bits and copy them into Speed } } } task BT_TxRx() { while (true) { // Send monitored values to Car SendToCar(); // Receive status information from Car ReceiveFromCar(); } } task ChangeDebug () // Wait for NXT Button (to enable/disable debug mode) // Use the ShowDebugInfo flag elsewhere in your program to dos Debug Things { bool tmp; tmp=ButtonCount(BTNCENTER, true); // Read & Clear pressed counter (in NXT) while (true) { Wait(100); if (ButtonCount(BTNCENTER, true)>0) { PlayTone(1760,50); Wait(100); ShowDebugInfo=!ShowDebugInfo; if (ShowDebugInfo) {PlayTone(1980,50);} } } } task StatusDisplay () // Show dashboard controls on NXT Display { #define BlinkCycle 600 // Dutty cylcle for blinking signals on display #define BlinkOn 300 // Must be less then total duty cycle time bool Blink=0; // Blink=1 = Show blinking symbol now! B_Time = CurrentTick()-BlinkCycle-1; // Set Blink Timer for Blink-function minus any value longer then blink duty cycle while (true) { if ((ReFuel) || (GoRight) || (GoLeft) || (Blink)) // If starting to blink or blinking busy { if (!Blink) //Blink is dimmed at the moment { if (B_Time99) { PwrCorrection=PwrPedal-100; PwrCorrection_OLD=PwrCorrection_OLD+PwrCorrection; } MotPwr=PwrPedal-PwrCorrection; if (PwrPedal-PwrCorrection>15) MotPwr=PwrPedal-PwrCorrection; else MotPwr=0; // Set power to zero during very low power levels (leaking currents in car) // Set PRND to Drive when giving power while not in Reverse // Set PRND to Neutral when releasing power while driving but not in Reverse if (PRND!=Reverse) { if (MotPwr>1) PRND=Drive; else { if (Speed>1) PRND=Neutral; else PRND=Park; } } // Monitor Steering Wheel (Rotation Sensor) and correct the reading using correction from previous reading SteerWheel=(-SENSOR_3 * SteerTick) - SteerCorrection_OLD; // Read steerwheel sensor // Check for too wide steering (would damage car) if (SteerWheel<-SteerEdgeMax+1) { SteerCorrection=SteerWheel+SteerEdgeMax; SteerCorrection_OLD=SteerCorrection_OLD+SteerCorrection; } if (SteerWheel>SteerEdgeMax-1) { SteerCorrection=SteerWheel-SteerEdgeMax; SteerCorrection_OLD=SteerCorrection_OLD+SteerCorrection; } SteerEdge=SteerWheel-SteerCorrection; // Activate Richtingaanwijzers when a sharp curve is made (more degrees then "FlashLight" if (SteerEdge<-FlashLight) GoRight=true; else GoRight=false; if (SteerEdge>FlashLight) GoLeft=true; else GoLeft=false; // Break & Reverse gear when S1 Pressed // S1 = Brake during driving // S1 = Reverse Gear or Park, when slow moving if (SENSOR_1) { if (Speed>25) {Brake=true; BrakePressed=true;} // Fast driving, emergency breaking nessesary else if (!BrakePressed) { BrakePressed=true; PlayTone(800,100); if (PRND!=Reverse) PRND=Reverse; else PRND=Neutral; } } else { Brake=false; BrakePressed=false; } // Monitor ReFuel Indicator (Battery Check) if (Fuel-1) { if (Speed<20) Speed=Speed-1; else Speed=Speed-3; MotPwr=Speed; Fuel=Speed; Wait(50); } Speed=0; ADC=true; GoLeft=1; GoRight=1; ReFuel=1; Wait(500); CPA=true; ADC=false; Wait(500); CPA=false; } start BT_TxRx; ShowDebugInfo=false; // Init procedure has finished, start other tasks on termination of Main task. }