Start

Förstasidan
Statistik

Social robot

Robotens syn
Modell/simulator
Levande modell


Arduino

Starta upp Arduino
DCcduino UNO
Pro Mini

Raspberry pi

Om Raspberry

Pic32

Om Pic32
Konstruktionen
Praktisk kom-igång
Starta upp UBW32
Installera MPLAB
Rinnande ljus
Stegmotor
Display
RS232 Pic32 - terminal
Luffarschack

Servo

Överblick
Isärmonterat servo
Styrning av servo
Mini Maestro
Bygga ett servo

Elektronik

Open Collector
Kontaktstuds
Drivsteg på utgångarna
Halleffektsensor/switch
MLX90316 (halleffekt)
Pulsbreddsmodulering

Open CV

Om OpenCV

Matematik

Linjär algebra
Olinjär länkmekanism

Servon
Vafför detta
Jag har nu tänkt mig göra ett eget servo i detta projekt, för att sköta rotationen i nacken, för att lära mig lite om "problemet".

Återkopplat system
Att bygga ett servo handlar oftast om att låta en motor snurra åt något håll och samtidigt lyssna på mätdata från en givare som ger återkoppling av positionen. När önskvärd position är uppnådd - då stannar man motorn där. Grovt skissat är det så det fungerar. Man litar alltså inte på motorn (motorn i ett typiskt servo är oftast en ganska primitiv dc-motor) men man litar på lägesgivaren. Det är lägesgivaren som får tala om ifall servot står i rätt läge.

Ett återkopplat system innebär alltså att man mäter och återkopplar resultatet, i detta fall via en positionssensor, för att kunna finjustera läget. Man kan jämföra det med att köra bil. Skall du svänga vänster vrider du lite på ratten samtidigt som tittar ut genom rutan (återkopplingen). Har du snurrat för lite på ratten vrider du lite till. Med synen (återkopplingen) lotsar du fram bilen dit du vill. Motsatsen till återkopplade system är öppna system. Vattenkranen är ett öppet system. Du vrider halvmycket och det kommer halvmycket vatten. Mer koll på läget än så har du inte. Öppna och återkopplade system benämns ibland open loop och closed loop.

Öppna system som är exakta och förutsägara
Man kan kort nämna ett ett viktigt undantag här. Det går att bygga ett mycket exakt och förutsägbart servo utan återkoppling med hjälp av en stegmotor. En stegmotor är en motor där all rotation styrs av att en dator styr motorns elektromagneter. Rotationen är därför väldigt exakt och förutsägbar. Det enda som behövs är att datorn håller räkningen på hur många steg den tickat motorn åt ena eller andra hållet. Det krävs också en möjlighet för datorn att nollställa systemet så att datorn får en startposition (det kan t.ex. vara en enkel switch som slås till i något ytterläge). Men i övrigt behövs ingen återkoppling och man slipper dessutom en del problem som alltid uppstår och måste hanteras i återkopplade system (läs mer om detta längre ner). Det kräver att stegmotorn dimensioneras så att den är tillräckligt stark för den last den skall styra, så att den inte "tappar steg". Denna typ av system är vanligt i t.ex. skrivare där matningen av pappret och rullar måste vara exakt synkroniserade med skrivelektroniken, speciellt i färgskrivare. Nackdelar med stegmotorer som servo är att de inte kan ganteras "hålla en position" då de inte känner av läget. Ett vanligt servo kompenserar direkt när det känner att det "tappat läget". Stegmotorer är ganska långsamma och svaga, speciellt vid högre varvtal. Om en stegmotor är en bra eller dålig lösning beror på omständigheterna. Nog om detta just nu.

Ingående delar
För att kunna styra motorerna exakt behövs alltså en givare, positionssensor, som talar om var motorn befinner sig. För detta ändamål har jag gjort lite expriment med en MLX90316. För att reglera hastighet och riktning på motorn tänkte jag använda mig av pulsbreddsmodulering.

Dvs, det ser ut ungefär som nedan.
effektsteg motor positions sensor
Regulator
Principen bakom regulatorn ser ut som nedanstående skiss. Vi skickar in ett börvärde (önskvärd position) i en komparator som låter skillnaden mellan BÖR och ÄR värde ligga till grund för hur motorn skall justeras. Regulatorn justerar då motorn så att skillnaden mellan ÄR och BÖR minimeras. Nu finns det ytterligare en parameter här som inte är helt oviktig och det är störsignalen i systemet. Mer om det längre ner

servo princip

Rigg
För att kunna labba lite med ovanstående skiss i praktiken har jag byggt en liten rigg. Nedan ser vi alltså en DC-motor med växellåda på vilken jag med hjälp av lite silikon limmat fast en magnet som i sin tur snurrar ovanför min MLX90316 -sensor. Nu har jag i detta expriment tagit en större magnet än vad jag senare kommer använda. Det beror på att jag inte har några mindre magneter tillgängliga just nu. Motorn är dessutom ansluten till ett slutsteg - kretsen L293 - och styrs som sagt med hjälp av pulsbreddsmodulering.

styra servo rigg

styra servo rigg

styra servo rigg

Ovanstående sladdinferno är alltså nedanstående inkoppling. När man vrider på potentiometern (till vänster, bör-värdet) skall alltså motorn göra motsvarande utslag (är-värdet) varken mer eller mindre. Återkommer till detta med mer eller mindre lite längre ner.
inkoppling
Eftersom positionssensorn ger ifrån sig en signal 0-4.5 volt och Pic32'ans max på ADC är 3.3 volt har jag lagt en liten spänningsdelare mellan MLX'n och Pic'n. Ingen exakt optimal lösning utan bara som ett skydd. I övrigt tänker jag mig att man får kalibrera systemet i mjukvaran. Ja, det är fortfarande bara på lab-nivå.

Mjukvara - Servo
Funktionsprincip
Mjukvaran som skall kontrollera ovanstående tar alltså ett börvärde som indata - i detta fall värdet via en potentiometer ansluten till UBW32 (i framtiden kommer börvärdet från en matematisk modell av roboten) - och får även in ett är-värde från lägesgivaren MLX90316. Beroende på om är-värdet är större eller mindre än bör-värdet så roterar processorn på DC-motorn åt ena eller andra hållet.

Om man tänker sig att man räknar positivt medurs får man följande enkla regler: Det verkar ju enkelt. Om sedan ÄR = BÖR så skall motorn stå stilla. Koden längst ner på sidan innehåller några rader som är hjärtat i hela regulatorn.

		BorVarde = ReadADC10(0);
		ArVarde = ReadADC10(1); 
		diff=BorVarde-ArVarde;
		dir=((diff<0)?2:1);

Dvs, vi läser av BörVärde och ÄrVärde från de analoga ingångarna och räknar sedan ut skillnaden (diff). Beroende på hur stor (absolutbeloppet av diff, dvs |diff|) skillnaden är snurrar vi olika fort på motorn (längre ner om varför) och beroende på om skillnaden är positiv eller negativ skickar vi ut pulståget på A0 eller A1 (kopplad till L293'an) och snurrar därmed åt ena eller andra håller på motorn.

Komplicerande omständigheter
Detta att ÄR = BÖR, dvs att motorn är i exakt rätt läge, inträffar väldigt sällan i praktiken p.g.a. flera orsaker av vilka några t.ex. är följande.
Man kan sammanfatta det med att det finns omständigheter och störsignal (en störparameter) i systemet som gör det jobbigt.

Mekaniskt glapp
kugghjul med glapp Till största delen är denna störparameter mekaniskt betingad p.g.a. glapp. Glapp finns överallt runt omkring oss. Det är sällan något problem, speciellt om vi slipper känna till det - som t.ex. glapp i bilens växellåda. Men glapp i reglersystem skapar problem, speciellt som glapp ibland tenderar att vara lite kaosartat. Betrakta kugghjulen till höger. Tänk dig att det finns en motor i punkt A. Det finns ett litet glapp mellan kugghjulen som blir större ju fler kugghjul konstruktionen har. Om det är lite glapp mellan varje kugghjul kan motorn i A snurra fram och tillbaka en liten bit utan att något händer i punkt B. Om vi har en positionssensor i punkt B som talar om hur motorn i punkt A ska justera läget så kommer ett fördröjningsproblem uppstå. Motorn A kommer vrida men givaren i B kommer inte omedelbart svara på denna rörelse. När sedan B svarar har A snurrat för långt och kommer få direktiv att istället snurra tillbaka. Det kommer bli självsvängning kring läget ÄR = BÖR. Vad ska man göra åt detta? Man kan jobba sig runt glappet både mjukvarumässigt och mekaniskt. Mekaniskt genom att t.ex. applicera en liten fjäder i punkt B. Men även något så enkelt som att smörja kugghjulen förbättrar situationen.

Mjukvarumässiga lösningar
En intressanta fråga är vad man kan göra mjukvarumässigt om skillnaden |ÄR-BÖR| är mycket lågt. Dvs, motorn är nästan i rätt position men inte riktigt.

Ett alternativ är att inte göra något speciellt. Filosofin här är då att om positionen inte är exakt - så är den inte. Att justera rätt position är ett evighetsjobb som ständigt pågår. Digitala hobbyservon "brummar" mer eller mindre konstant. Vissa oroas lite av att det låter hela tiden, men detta är helt normalt och det beror just på detta att servot nästan aldrig är i rätt position helt exakt. Därför finns det alltid någon riktning som servot vill justera sig i, även om det är oerhört små vinklar. Ett problem här är förutom att det brummar lite - vilket iofs inte behöver vara ett problem, det kan lika gärna vara hemtrevligt - så drar det mer ström än om motorn stod stilla. Det producerar också mer värme, både i motorn och i slutsteget. Det behöver iofs inte heller vara något problem. Ett tänkbart mer allvarligt problem är om motorn hamnar i svängning mellan två lägen kring bör-värdet. En svängning som uppstår p.g.a. oexakta leder. Eftersom motorerna trots sin ringa storlek är rätt så starka kan man tom tänka sig scenariot att motorerna skakar sönder konstruktionen. Och det vore ju lite tråkigt.

Problemet med glapp i konstruktionen löser jag med följande kodrader:

		if(ABS(diff)<6)
		{
			puls_uppe=0;
		}

Dvs, jag stänger av motorn om skillnaden mellan är-värde och bör-värde är 6 eller mindre. Detta är i min testrigg. Det motsvarar 1-2 graders vinkel. I den skarpa miljön kommer glappet minskas lite p.g.a. lämpligt placerade fjädrar och naturliga krafter (p.g.a. tyngdkraften) och då kommer denna spalt förmodligen halveras. Mer exakt behöver det inte vara.

Översläng
inkoppling

Eftersom allt som tvingas/bringas i rörelse får en rörelseenergi p.g.a. massan så måste rörelsen "bromsas" på ett kontrollerat sätt för att stanna i rätt läge.

En tumregel är att ju högre krav man har på ett snabbt svar vid en förändring desto mer problem med översläng. Minst problem med översläng har man i långsamma system där det inte är så viktigt hur snabbt svaret är vid en förändring. Matematiken kring detta - och inte minst analysen - kan snabbt bli mycket komplicerad. Men man behöver inte krångla till det mer än nödvändigt. Åtminståne inte i första versionen.

Problemet med översläng löses helt sonika med följande rader där den översta hälften gör nästan hela jobbet. Vad koden åstadkommer är att hastigheten på motorn minskas om skillnaden mellan ärvärde och börvärde är mindre än 25 vilket motsvarar ca 9 graders vinkel. Hastigheten minskas mer ju närmare börvärdet motorn kommer och motorn stängs helt av ifall vinkeln är närmare än 1.8 grader.

		if(ABS(diff)<25)
		{
			puls_uppe=ABS(diff*2);
		}
		if(ABS(diff)<6)
		{
			puls_uppe=0;
		}

Detta trollade helt bort överslängen i testriggen. OBS! När servot placeras i den skarpa miljön (roboten) kommer man få återvända till detta problem eftersom servot då styr en större massa med större tröghet. Men strunt i det just nu.

Filter
Man skulle kunna frestas att "trolla bort" mindre störningar - störningar som "känns" som de är högfrekventa - med ett lågpassfilter. T.ex. genom att ta snittvärdet på de senaste resultaten från positionssensorn. Detta fungerar inte. Ett lågpassfilter får som effekt att motorn pendlar fram och tillbaka med 1-3 Hz. Ett högpassfilter sätter hela styrningen ur funktion. Servot kommer snurra runt samtidigt som servot skakar nervöst. Detta med filter är extremt komplicerad materia och inget för hemmasnickraren med ganska små behov.

Stabil styrning
Att få ett servo röra sig snabbt och smidigt verkar koka ner till att dels minimera allt glapp mellan motor och positionssensor och dels hantera den överslängen som kan uppstå p.g.a. hög hastighet eller stor massa.

Kod till servot
/********************************************************************
Regulator1 på UBW32              -         http://www.robotsteel.com
********************************************************************/

#include "plib.h"
#include "HardwareProfile.h"

#define FREK           		3000
#define SYS_FREQ 			(80000000L)
#define TOGGLES_PER_SEC		5120
#define CORE_TICK_RATE	    (SYS_FREQ/2/(TOGGLES_PER_SEC*FREK/100))

#define PARAM1  	ADC_MODULE_ON | 
			ADC_FORMAT_INTG | 
			ADC_CLK_AUTO | 
			ADC_AUTO_SAMPLING_ON
#define PARAM2  	ADC_VREF_AVDD_AVSS | 
			ADC_OFFSET_CAL_DISABLE | 
			ADC_SCAN_ON | 
			ADC_SAMPLES_PER_INT_2 | 
			ADC_ALT_BUF_OFF | 
			ADC_ALT_INPUT_OFF
#define PARAM3  	ADC_CONV_CLK_INTERNAL_RC | 
			ADC_SAMPLE_TIME_15
#define PARAM4		ENABLE_AN4_ANA | 
			ENABLE_AN5_ANA
#define PARAM5		SKIP_SCAN_AN0 | 
			SKIP_SCAN_AN1 | 
			SKIP_SCAN_AN2 | 
			SKIP_SCAN_AN3 | 
			SKIP_SCAN_AN6 | 
			SKIP_SCAN_AN7 | 
			SKIP_SCAN_AN8 | 
			SKIP_SCAN_AN9 | 
			SKIP_SCAN_AN10 | 
			SKIP_SCAN_AN11 | 
			SKIP_SCAN_AN12 | 
			SKIP_SCAN_AN13 | 
			SKIP_SCAN_AN14 | 
			SKIP_SCAN_AN15

#define ABS(a) (((a) < 0) ? -(a) : (a))

int puls_uppe=0,puls_langd=0;
unsigned short int BorVarde, ArVarde;
int diff, dir;

int main(void)
{   
	SYSTEMConfigPerformance(80000000L);
 	mJTAGPortEnable(0);

	CloseADC10();
	SetChanADC10( ADC_CH0_NEG_SAMPLEA_NVREF);
	OpenADC10( PARAM1, PARAM2, PARAM3, PARAM4, PARAM5 );
	EnableADC10(); 

	mInitAllLEDs();
	TRISA = 0;

	OpenCoreTimer(CORE_TICK_RATE);
	mConfigIntCoreTimer((CT_INT_ON|CT_INT_PRIOR_2|CT_INT_SUB_PRIOR_0));
	INTEnableSystemMultiVectoredInt();
	// vänta på första konverteringen blir färdig
	while ( ! mAD1GetIntFlag() ) { }  
	while(1) ;
	return 0;
}

void __ISR(_CORE_TIMER_VECTOR, ipl2) CoreTimerHandler(void)
{
	mCTClearIntFlag();
	if(puls_uppe<0)
	{
		dir=0;
	}
	PORTA=dir;
	puls_langd--;
	puls_uppe--;
	if(puls_langd<0)
	{
		BorVarde = ReadADC10(0);
		ArVarde = ReadADC10(1); 
		diff=BorVarde-ArVarde;
		dir=((diff<0)?2:1);
		puls_uppe=100; // fullfart
		puls_langd=100;

		if(ABS(diff)<25)
		{
			puls_uppe=ABS(diff*2);
		}
		if(ABS(diff)<6)
		{
			puls_uppe=0;
		}
	}
	UpdateCoreTimer(CORE_TICK_RATE);
}





Robot och elektronik -sidan byter namn till electro.st

Senast läst 21:28:11 18/7 2017 Texten uppdaterad 17/9 2014
footer sign