Programeringsfrågor C++ radiostyrd båt

C, C++, Pascal, Assembly, Raspberry, Java, Matlab, Python, BASIC, SQL, PHP, etc.
Glattnos
Inlägg: 2972
Blev medlem: 29 oktober 2009, 20:01:18

Re: Programeringsfrågor C++ radiostyrd båt

Inlägg av Glattnos »

För att få bukt med kompass-sensorn skrev jag följande kod som körs medan man kör båten runt ett kalibrerings-varv. Då jag flyttat mig i Sverige så verkar jag få helt andra värden än tidigare så denna kod kalibrerar mitt-punkten på X och Y värdena så att dom hamnar på 0 och det funkar jättebra faktiskt såhär långt.
Min och Max-värdena initieras som högt positivt och lågt negativt för att ta emot MagX och MagY, som där jag befinner mig nu inte alltid går över 0:an.

Kod: Markera allt

int x;
	I2C1_Read();
	if(!(Auto_Initialized)){
		if(MagX > MagX_Max) MagX_Max = MagX;
		if(MagX < MagX_Min) MagX_Min = MagX;
		if(MagY > MagY_Max) MagY_Max = MagY;
		if(MagY < MagY_Min) MagY_Min = MagY;
	}
	DirectionX = MagX - (MagX_Min + ((MagX_Max - MagX_Min) / 2));
	DirectionY = MagY - (MagY_Min + ((MagY_Max - MagY_Min) / 2));
	x = (atan2(DirectionY,DirectionX) * 180 / 3.14159265359) + 180;
	if(x >= 360) x -= 360;
	else if(x < 0) x += 360;
	Direction = x;
Nått jag funderar lite på är om man kan "dämpa" lite på utslagen. Jag provade att läsa in 8 st värden(med 40Hz frekevens) i en buffer och sen lägga ihop dom och dela med 8. Detta funkar bra...men inte när det ligger vid 359-0 grader, då räknas det ju upp och ner beroende på om det ligger på 0 eller 359.
Finns det något klurigt sätt att lösa detta?
Användarvisningsbild
lillahuset
Gått bort
Inlägg: 13969
Blev medlem: 3 juli 2008, 08:13:14
Ort: Norrköping

Re: Programeringsfrågor C++ radiostyrd båt

Inlägg av lillahuset »

Det naturliga är väl att lägga valfri offset på vinkeln när du räknar. Tex 1000..1359.
Glattnos
Inlägg: 2972
Blev medlem: 29 oktober 2009, 20:01:18

Re: Programeringsfrågor C++ radiostyrd båt

Inlägg av Glattnos »

Jag provade det men det blir ju samma problem. Lägger man till 1000 så kommer buffern att fyllas med blandade 1000 och 1359 när man ligger runt 0. Eller missar jag någonting?
Användarvisningsbild
lillahuset
Gått bort
Inlägg: 13969
Blev medlem: 3 juli 2008, 08:13:14
Ort: Norrköping

Re: Programeringsfrågor C++ radiostyrd båt

Inlägg av lillahuset »

Nej, jag tänkte fel. :oops:

Jag har gjort exakt den här saken för en herrans massa år sedan, vi mätte obalansen i en roterande maskin, och jag hade för mig att det var så jag gjorde. Men det var det nog inte...
Jag får fundera och återkomma om ingen annan kommer med ett bra svar innan.
farskost
Inlägg: 454
Blev medlem: 17 april 2006, 19:55:21
Ort: Mölndal

Re: Programeringsfrågor C++ radiostyrd båt

Inlägg av farskost »

Det här är en intressant metod, blir ett par beräkningar men kanske är värt det:

https://stackoverflow.com/questions/499 ... ngle-value

https://en.m.wikipedia.org/wiki/Mean_of ... quantities


En annan lösning kanske skulle vara att spara undan vinkeln för första mätningen och sedan bara spara undan skillnaden mellan denna vinkel och efterföljande mätning, för att sedan beräkna medelvärdet av dessa och lägga till eller dra från ditt sparade första värde.

Om du behöver inspiration till kod för att hantera differenser mellan vinklar finns det lite matnyttigt här: https://stackoverflow.com/questions/114 ... -in-c-code

Annars kanske du kan beräkna medel på dina mätningar i X och Y för att sedan beräkna vinkeln från det filtrerade värdet?
Glattnos
Inlägg: 2972
Blev medlem: 29 oktober 2009, 20:01:18

Re: Programeringsfrågor C++ radiostyrd båt

Inlägg av Glattnos »

Tack så mycket! Att filtrera X och Y var för sig var ju en utmärkt ide :)
Eftersom X och Y pendlar mellan MIN och MAX så blir det ju inga "hopp" i dom siffrorna. Denna kod verkar funka bra och ger en jämn och vad jag kan bedöma ganska exakt kurs.

Kod: Markera allt

	int x;
	I2C1_Read();
	MagX_Buffer[Mag_Buffer_Index] = MagX;
	MagY_Buffer[Mag_Buffer_Index] = MagY;
	if(Mag_Buffer_Index >= BUFFSIZE_MAG-1) Mag_Buffer_Index = 0;
	else Mag_Buffer_Index++;
	int y = 0;
	for(int a = 0; a < BUFFSIZE_MAG; a++) y += MagX_Buffer[a];
	MagX = y / BUFFSIZE_MAG;
	y = 0;
	for(int a = 0; a < BUFFSIZE_MAG; a++) y += MagY_Buffer[a];
	MagY = y / BUFFSIZE_MAG;
	if(!(Auto_Initialized)){
		if(MagX > MagX_Max) MagX_Max = MagX;
		if(MagX < MagX_Min) MagX_Min = MagX;
		if(MagY > MagY_Max) MagY_Max = MagY;
		if(MagY < MagY_Min) MagY_Min = MagY;
	}
	DirectionX = MagX - (MagX_Min + ((MagX_Max - MagX_Min) / 2));
	DirectionY = MagY - (MagY_Min + ((MagY_Max - MagY_Min) / 2));
	x = (atan2(DirectionY,DirectionX) * 180 / 3.14159265359) + 180;
	if(x >= 360) x -= 360;
	else if(x < 0) x += 360;
	Direction = x;
Det är synd att det blåser så mycket för jag skulle vilja testa båten på sjön. Som den är nu så har den manuellt läge när det styrs som vanligt med en sändare. Man får manuelt köra runt ett varv för att kalibrera kompassen. Sen är tanken:
Rikta in åt vilket håll den ska starta och slå på Auto-läge, då tar den en GPS-position och följer sen kompasskursen i 15 sek(ställbar parameter) för att sedan ta ytterligare en GPS-position. Den linjen som skär dessa två positioner blir den linje den sedan följer, den kommer alltså att kompensera för sidvind och vågor för att hålla linjen positions-mässigt med GPS:en.

Jag har valt att inte programmera Waypoints(som den siktar på och åker emot) som många andra verkar göra. Istället sätter jag en "backpoint" och en kurs. Då kan systemet när som helst sätta sin nuvarande position som backpoint och en kurs utifrån detta. Men största fördelen som jag ser det är egentligen att man kan sätta en backpoint var som helst, tex 40 meter parallellt bredvid aktuell linje och kursen 180 grader emot aktuell kurs. Då kommer båten att försöka ta sig till den linjen och kommer sen följa den åt andra hållet, oavsett när man sätter den nya linjen eller var båten befinner sig när man gör det. Om det skulle ligga land i vägen så kan båten med hjälp av djup-data och avstånds-sensor navigera sig förbi utan att det finns en "helig waypoint" som den måste nå. En backpoint kan ligga på land utan att det blir problem. Försöker illustrera med en bild :)

Just det sättet att göra det på har jag inte läst någonstans utan det bara föll sig så att jag tyckte det verkade smidigt. Det kan ju såklart användas mycket, även om inte jag har sett det, det brukar vara så :)
Du har inte behörighet att öppna de filer som bifogats till detta inlägg.
Glattnos
Inlägg: 2972
Blev medlem: 29 oktober 2009, 20:01:18

Re: Programeringsfrågor C++ radiostyrd båt

Inlägg av Glattnos »

Två problem har jag stött på som båda verkar kunna ha att göra med fartreglaget.
Problem 1:
Båten har varit väldigt inkonsekvent på riktningen, den har verkat köra spikrakt när den väl har fått jämn fart men har varit mycket "orolig" IBLAND när den ska svänga eller byta hastighet. Jag har letat som en tok i koden efter vad som kan vara fel men inte hittat någon förklaring till de olika beteendena. Tills nu...jag började notera vilka riktningar den tar istället för det den borde tagit och kom fram till att det beror på om det är X eller Y från magnetometern som har störst inflytande. Framåt är det Y och när man backar är det X. Då kom jag på att jag inte kollat på den framräknade riktningen när jag gasar. Gjorde det och såg då att om båten pekar åt norr (0 grader) eller åt syd (180 grader) så påverkas inte riktningen när man gasar framåt. Står båten däremot i någon öst-västlig riktning så vrider kursen olika mycket beroende på gaspådrag. Om jag gasar bakåt istället så är det tvärt om, då är det nors/syd riktningar som vrider och öst/västliga riktningar som är mindre påverkade.
Typiskt...att jag inte kollade detta tidigare. Nånting stör förmodligen magnetometern när jag gasar. Fartreglaget är ett sådant: http://www.hobbyequipment.se/tillbehor- ... glage.html
Det justerar ju med PWM till motorn som är en induktiv last vilket borde skapa elektomagnetism.
Frågan är vad som är mest troligt. Kan reglaget i sig, som sitter ca 5 cm ifrån magnetometern skapa störningarna? Är det själva motorn, som sitter under båten som stör? Eller kan kablarna störa?

Problem 2:
Mitt fartreglage blinkade långsamt rött och reagerade inte trots att det funkat i flera dagar och trots uprepade kalibreringar. Enligt manualen blinkar det så när det är i säkerhetsläge med antingen låg in-spänning volt eller hög temperatur i reglaget. Reglaget är helt kallt så det kan knappast vara det. Volten kan inte heller vara för låg, dock för hög. Det är ju ämnat för 3S LiPo(11,1 nom) och jag kör med vanligt fritidsbatteri (12V). Jag har dock solcellsladdning och det gjorde att spänningen över batteriet och fartreglaget var 14,3V vid tillfället. För att testa så kopplade jag ur själva solcellspanelen och fick då 13,1V istället. DÅ fungerade reglaget. Jag har stängt av LowVoltage CutOff-funktionen.
Är det alltså även skydd emot överspänning i reglaget som inte står någonstans i manualen och inte går att stänga av?

Jag ska börja med att flytta reglaget en bit ifrån elektronik-kortet och testa. Är det någon som har något mer förslag på vad jag kan göra åt respektive problem? Förslag på annat fartreglage?
Glattnos
Inlägg: 2972
Blev medlem: 29 oktober 2009, 20:01:18

Re: Programeringsfrågor C++ radiostyrd båt

Inlägg av Glattnos »

En Biltema elutombordare lär väl inte ha så mycket avkoppling emot störningar antar jag. Jag har läst denna sida: http://www.stefanv.com/rcstuff/qf200005.html
Men måste man sätta kondensatorer och induktanser nära motorn för att få bra effekt eller kan man sätta dom på kablarna innan dom går ner i vattnet? Själva motorn sitter ju under vattnet. Det kan gå att öppna den och pilla in kondensatorer i den men det vill jag undvika om det går.
Glattnos
Inlägg: 2972
Blev medlem: 29 oktober 2009, 20:01:18

Re: Programeringsfrågor C++ radiostyrd båt

Inlägg av Glattnos »

Testade att flytta fartreglaget och det funkade faktiskt ganska bra. Istället för att kompassen vrider ca 70-80 grader vig gaspådrag så verkar det vrida sig max 10 grader nu. Väldigt bra men inte perfekt så ytterligare förbättringar är välkomna :)
Användarvisningsbild
Andax
Inlägg: 4373
Blev medlem: 4 juli 2005, 23:27:38
Ort: Jönköping

Re: Programeringsfrågor C++ radiostyrd båt

Inlägg av Andax »

Om det är magnetfältet från kraftkablarna till och från fartreglaget/motorn som stör kompassen så kan du kanske försöka hålla ihop plus och minuskablar (tvinna lätt kanske) för magnetfälten från dessa kablar borde ta ut varandra om de ligger tätt.
farskost
Inlägg: 454
Blev medlem: 17 april 2006, 19:55:21
Ort: Mölndal

Re: Programeringsfrågor C++ radiostyrd båt

Inlägg av farskost »

Finns det möjlighet att flytta kompassen? På en liten bom fram i fören eller på toppen av en liten mast för att komma bort från störkällor och annan metall?
xxargs
Inlägg: 10183
Blev medlem: 23 september 2006, 14:28:27
Ort: Södertälje

Re: Programeringsfrågor C++ radiostyrd båt

Inlägg av xxargs »

Andax skrev:Om det är magnetfältet från kraftkablarna till och från fartreglaget/motorn som stör kompassen så kan du kanske försöka hålla ihop plus och minuskablar (tvinna lätt kanske) för magnetfälten från dessa kablar borde ta ut varandra om de ligger tätt.
om du gör fyr-skruv (+ resp - diagonalt gentemot varandra i 4-bunten) så minskar du fältet ganska mycket mera gentemot 'bara' en parledning.

som sagt framledare och returledare skall hållas så nära varandra det går - ju mindre area mellan dem, ju mindre med synlig magnetfält utanför - och i en fyr-skruv får man kompensation av varandra i två axlar.
Glattnos
Inlägg: 2972
Blev medlem: 29 oktober 2009, 20:01:18

Re: Programeringsfrågor C++ radiostyrd båt

Inlägg av Glattnos »

Många bra tips. Man tackar så mycket :)
Igår gick ändå båten rätt så bra, men vid en senare uppgradering ska jag nog kombinera alla era förslag. Jag har satt extra I2C anslutning på kortet just för att kunna ha extern kompass. Och kablarna kan bytas mot tvinnade, det är tvåledare nu.
Båten gick helt själv i drygt en timme igår(tills jag stängde av den), jag följde bara efter och övervakade. Jag gissar att kompassen felade ca 15-20 grader som mest eftersom den då och då fick styra tillbaka för att hålla linjen. Jag tror nästan att det skulle räcka med lite aggressivare kompensering för att få den att gå rakt men istället då lite bredvid linjen.
Jag hade vändning på 3,5 m djup igår eftersom jag inte riktigt fått Lidarn att funka så bra som jag vill ännu. Men tanken är att båten senare ska kunna operera på vatten med ca 1,5 m djup.

Lidarn är en Garmin Lidar-Light v3 som jag försöker få att funka i PWM-läge. Allt är kopplat enligt manualen men hittar ingen riktigt bra beskrivning på hur man kodar. Det är en kabel som är både trigger och signal.
Glattnos
Inlägg: 2972
Blev medlem: 29 oktober 2009, 20:01:18

Re: Programeringsfrågor C++ radiostyrd båt

Inlägg av Glattnos »

Jag har inte lyckats få programmet att ge konsekventa avstånd från Lidarn och har idag försökt gå till botten med problemet. Det är alltså en Lidar Lite v3HP: https://www.robotshop.com/eu/en/lidar-l ... lv3hp.html

Man kan alltså använda snesorn i PWM-läge om man sätter ett 1K pull-down motstånd på mode-ledningen. Då skickar sensorn en PWM-signal på samma ledning där pulslängden motsvarar avståndet med 10us/cm eller 1ms/meter. Jag kopplade oscilloskopet på signalen och såg då att den ger rätt pulslängd konsekvent upp till drygt 20 meter(ca 21 meter) men om jag mäter på nått längre bort så blir signalen instabil och börjar direkt att hoppa omkring längdmässigt, man ser allt ifrån 1-39 ms längd. Det är helt omöjligt att få den stabil på ett värde mellan 21-39ms. Pekar man den rakt upp i luften så beter den sig likadant även om man borde kunna förvänta sig en pulslängd på runt 40ms. Den går att hålla stabil på alla värden under 21ms(även med hög upplösning).

När man kollar specen på sensorn så ska den klara upp till 40 meter och med relativt hög presicion också. Är denna typ av laser-sensor så "lynnig" på längre avstånd eller kan det vara nått fel på min givare?
Glattnos
Inlägg: 2972
Blev medlem: 29 oktober 2009, 20:01:18

Re: Programeringsfrågor C++ radiostyrd båt

Inlägg av Glattnos »

Har nu plockat loss givaren från båten och testat den enbart med labbaggregat på 5.0V, pull-up motstånd och oscilloskop och den beter sig likadant. Sensorn skickar en fin PWM-signal upp till drygt 20 meter (20ms) men därefter blir det blandade pulslängder.
Måste ju vara fel på sensorn.
Skriv svar