Anyone wanna take a guess at this one?

[wannaBinventor]

I've got some EFLRDS75H digital servos that I picked up from a local hobby shop to play with. I'm using a PIC16F1934 and an MMA7731 accelero to measure g force. Long story made short, I'm deriving the angle of inclination of the X and Y axis in respect to the direction of acceleration (gravity) and then feeding that into some other calculations to create appropriate pulses on the servos so that they match the angle at which I'm inclining my accelerometer board. The servos are connected via some improvised mount I made where a little platform stays on roughly the same plane as my accelero board.

I've been playing with it for the last little while and I had it running fine. I could pick it up and move it around with NO VIBRATION.

In the last 10 minutes, it started shaking randomly, as if it was jumping between two angles at, well, 25hz I guess. When I move it around, I think the one on the Y axis is the one that is shaking. I've also got an LCD set up to display G force and angle and it also shows 0.1g variation (which is the max resolution in my program's calculations) on all axis, but I think this is just picking up vibrations from the table which in turns puts it into an endless feedback loop until I pick it up or put my finger on the servo.

Does anyone have an idea why, all of the sudden, without any changes to anything, it would go from moving linearly through the (coarse) angle steps, to suddenly shaking through the steps and also occasionally beginning to shake while just sitting on the table?

For what it's worth, I don't see any variation in the power supply. It's running through a decoupled LM317. I've also reset the program, and reflashed it for giggles.

 

[Mr RB]

I think that the really coarse ADC resolution you are using of 0.1 G is causing instability. If you want to measure angle and get closed-loop control of the angle you need much better resolution. As mentioned in your other thread.

 

[MikeMl]

Cheap Servos? The feedback pot inside them gets noisy.

 

[wannaBinventor]

Possibly. They were 11 bucks each from the hobby store, which would probably make them 7 or so at a place like jameco. The thing is though, these are stock servo's inside at least one model of RC helicopter, and they are digital, so I'd assume they have to be better.

Either way, I solved most of my problem. For one, I had a problem with my calculations at the 0g crossover. I added a hundreths place to the display and to the other calculations, and I could see if I used a wadded up napkin as a fulcrum and just slowly tilted it in from .01g to 0 g, the servos would go to the extreme left of it's range. I guess when it was sitting there it was maybe picking up tiny vibrations and/or wandering. I just don't understand why this didn't present itself before.

Also, there were only 20ish measurements in my arc cosine lookup table. As you guys probably know, as acos approaches 1 and -1, it's slope steepens quickly. With only tenths place accuracy, the first step is about 26 degrees! Now using hundreths place accuracy, it is still a big jump from 0 (8 degrees), but it flattens pretty quickly.

I also hot glued a dead 9volt to the mount and it seems to help mitigate vibration by slowing down the servo I assume.

 

[Mike - K8LH]

How are you driving multiple servos? Could there be a problem with resolution or jitter there?

 

[wannaBinventor]

;THIS IS THE INTERRUPT SERVICE ROUTINE WHICH GETS VISITED EVERY 10ms FOR THE SERVO SIGNAL LINE "CLEAR PERIOD"
;AND THEN TURNS TMR2 (WHICH CREATED THE 10ms DELAY) OFF, AND THEN LOADS IN THE CALCULATED VALUE FOR TMR1
;TO SET THE APPROPRIATE TIME TO HOLD THE SERVO SIGNAL LINE HIGH, AND TURNS IT ON.  AFTER THAT TIME ELAPSES,
;IT CLEARS THE SIGNAL LINE FOR ANOTHER 20ms
;IN ACTUALITY, IT ALTERNATES BETWEEN SETTING EACH OF THE TWO SERVO'S HIGH EVERY 10ms, SO ANY ONE PARTICULAR SERVO ONLY SEES A 
;PULSE EVERY 20ms

ISR
	INCF		XYALTERNATE
	BANKSEL		PIR1			;SELECT PERIPH INT FLAG REGISTER 1
	BTFSC		PIR1,TMR2IF		;TEST TMR2 PR2 REGISTER MATCH INT FLAG
	GOTO		START_TMR1		;if tmr2 was source of int, then 20ms has passed and need to send servo a pulse
	BTFSC		PIR1,TMR1IF		;TEST FOR TMR1 OVERFLOW INT FLAG
	GOTO		START_TMR2		;if tmr1 was source, then the pulse is over and we need to start tmr2 back up to count 20ms

START_TMR1
	BCF			PIR1,TMR2IF		;CLEAR THE TMR2 PR2 REG MATCH INT FLAG

	MOVLW		.1
	SUBWF		XYALTERNATE,W
	BTFSC		STATUS,Z		;TESTS TO SEE IF XYALTERNATE IS 1, IF IS IT SETS UP X AXIS FOR SERVO PULSE
	GOTO		SETXAXISPULSE	
	GOTO		SETYAXISPULSE			

SETXAXISPULSE
	BCF			PORTD,0
	BSF			PORTC,1			;SET PORTC BIT 1 HIGH TO CREATE PULSE ON X AXIS SERVO (GIVEN TMR CREATED DELAY)
	BCF			T2CON,TMR2ON	;TURN OFF TMR2 SO THE PULSE CODE CAN RUN APPROPRIATELY

	MOVF		TMR1XHOLD_L,W
	MOVWF		TMR1L
	MOVF		TMR1XHOLD_H,W
	MOVWF		TMR1H
	BSF			T1CON,TMR1ON	;TURN ON TMR1 AFTER HAVING PRELOADED THE COUNTER
	GOTO		ENDISR


SETYAXISPULSE
	BCF			PORTC,1			
	BSF			PORTD,0			;SET PORTC BIT 0 HIGH TO CREATE PULSE ON Y AXIS SERVO (GIVEN TMR CREATED DELAY)
	BCF			T2CON,TMR2ON	;TURN OFF TMR2 SO THE PULSE CODE CAN RUN APPROPRIATELY
	CLRF		XYALTERNATE		;CLEAR THE XYALTERNATE FILE SO THE NEXT PULSE WILL BE ON THE X AXIS SERVO

	MOVF		TMR1YHOLD_L,W
	MOVWF		TMR1L
	MOVF		TMR1YHOLD_H,W
	MOVWF		TMR1H
	BSF			T1CON,TMR1ON	;TURN ON TMR1 AFTER HAVING PRELOADED THE COUNTER
	GOTO		ENDISR

START_TMR2	;TIMER 2 IS FOR GENERATING THE 10 ms WINDOW
	BCF			PIR1,TMR1IF		;CLEAR TIMER 1 INTERRUPT FLAG
	BCF			PORTD,0			;STOP HI SIGNAL TO Y AXIS SERVO
	BCF			PORTC,1			;STOP HI SIGNAL TO X AXIS SERVO
	BCF			T1CON,TMR1ON	 ;TURN OFF TIMER1 OR IT WILL INTERRUPT IN THE MIDDLE OF THE 20MS COUNT
	BSF			T2CON,TMR2ON 	;TURN ON TIMER 2 SO IT CAN START TIMING FOR 10MS AGAIN	
	BTFSS		XYALTERNATE,1		;IF AXISCOUNT IS ALREADY 2 (AS IT BIT 1 SET), SKIP NEXT
	CLRF		XYALTERNATE
	
ENDISR
	RETFIE

The TMR1YHOLD and TMR1XHOLD files are calculated in the main program. I'm working on something right now to load the "TMR1HOLD" files based on a rolling average of 10 ADC readings that I think should further supress the little bit of vibration that's left.

 

[skyhawk]

If you are looking to compute the angle of inclination, you need to be using the accelerometer channel perpendicular to the one you are using. Then your reading will be proportional to sin of the angle. For angles near zero, sin theta is approximated by theta in radians.

 

[Mike - K8LH]

I don't see anything wrong with your servo ISR code. Actually, using 10-msec Timer 2 interrupts for two servos is pretty insightful.

May I ask why you placed the servos on RC1 and RD0 instead of on two consecutive port pins? Also, why are you turning Timer 2 off and on? You're outputting the new servo channel pulse and setting up and turning on Timer 1 during the Timer 2 interrupt and then turning off the servo channel pulse and turning off Timer 1 after the Timer 1 interrupt. The Timer 1 interrupts will occur long before the next 10-msec Timer 2 interrupt so there's no reason not to leave Timer 2 running. If you're interested, here's my interpretation of your code with servo outputs on RD0 & RD1;

;
;  void interrupt()             //
;  { if(pir1.TMR2IF)            // if 10-msec TMR2 interrupt
;    { pir1.TMR2IF = 0;         // clear TMR2 interrupt flag
;      portd |= channel;        // output new channel pulse
;      if(portd.1)              // if RD0 channel on
;        tmr1 = xhold;          // use 'xhold' for RD1
;      else                     // else
;        tmr1 = yhold;          // use 'yhold' for RD0
;      t1con.TMR1ON = 1;        // turn TMR1 on
;    }
;    else                       // process the TMR1 interrupt
;    { pir1.TMR1IF = 0;         // clear TMR1 interrupt flag
;      portd &= 0b11111100;     // turn RD0 & RD1 outputs off
;      t1con.TMR1ON = 0;        // turn TMR1 off
;      channel ^= 3;            // toggle channel select bits
;    }
;  }
;
ISR
        banksel PIR1            ; bank 0                                  |B0
T2Proc
        btfss   PIR1,TMR2IF     ; TMR2 interrupt? yes, skip, else         |B0
        bra     T1Proc          ; branch                                  |B0
        bcf     PIR1,TMR2IF     ; clear TMR2 interrupt flag               |B0
        movf    PORTD,W         ;                                         |B0
        iorwf   Channel,W       ; Channel = 00000010 or 00000001          |B0
        movwf   PORTD           ; output new RD0 (Y) or RD1 (X) pulse     |B0
        movf    XHOLD_L,W       ;                                         |B0
        btfsc   PORTD,0         ;                                         |B0
        movf    YHOLD_L,W       ;                                         |B0
        movwf   TMR1L           ;                                         |B0
        movf    XHOLD_H,W       ;                                         |B0
        btfsc   PORTD,0         ;                                         |B0
        movf    YHOLD_H,W       ;                                         |B0
        movwf   TMR1H           ;                                         |B0
        bsf     T1CON,TMR1ON    ;                                         |B0
        retfie                  ;                                         |B0
T1Proc
        bcf     PIR1,TMR1IF     ; clear TMR1 interrupt flag               |B0
        movf    PORTD,W         ;                                         |B0
        andlw   b'11111100'     ;                                         |B0
        movwf   PORTD           ; turn RD0 & RD1 outputs off              |B0
        bcf     T1CON,TMR1ON    ; turn timer 1 off                        |B0
        movlw   3               ;                                         |B0
        xorwf   Channel,F       ; Channel = 00000001 or 00000010          |B0
        retfie                  ;                                         |B0
;

 

[wannaBinventor]

Skyhawk: Thanks for that tip. I'm far from the mathematician. In fact, I'm just an accountant who is decent with algebra and a little calculus, but as you can imagine, there isn't a whole lot of trig done in my field. I'm not sure I completely understand what you mean by using the channel perpendicular to the one I'm using. For instance, right now I'm calculating the arc cosine of the g force on either the X or Y axis divided by the total force vector, which should really always be extremely close to one since this is a more or less stationary setup. As such, it just simplifies to the arc cosine of the axis, and X and Y are both perpendicular to the direction of gravity at 0 g. So Y axis degree in respect to direction of gravity = arc cos (Yaxisg / 1). Are you saying I should be using "Y axis degree = arc sin (Xaxisg / 1). Correct me if I'm wrong (I probably am), but I don't see that doing anything but inverting my LU table.

I don't see anything wrong with your servo ISR code. Actually, using 10-msec Timer 2 interrupts for two servos is pretty insightful.

May I ask why you placed the servos on RC1 and RD0 instead of on two consecutive port pins? Also, why are you turning Timer 2 off and on? You're outputting the new servo channel pulse and setting up and turning on Timer 1 during the Timer 2 interrupt and then turning off the servo channel pulse and turning off Timer 1 after the Timer 1 interrupt. The Timer 1 interrupts will occur long before the next 10-msec Timer 2 interrupt so there's no reason not to leave Timer 2 running. If you're interested, here's my interpretation of your code with servo outputs on RD0 & RD1;

;
;  void interrupt()             //
;  { if(pir1.TMR2IF)            // if 10-msec TMR2 interrupt
;    { pir1.TMR2IF = 0;         // clear TMR2 interrupt flag
;      portd |= channel;        // output new channel pulse
;      if(portd.1)              // if RD0 channel on
;        tmr1 = xhold;          // use 'xhold' for RD1
;      else                     // else
;        tmr1 = yhold;          // use 'yhold' for RD0
;      t1con.TMR1ON = 1;        // turn TMR1 on
;    }
;    else                       // process the TMR1 interrupt
;    { pir1.TMR1IF = 0;         // clear TMR1 interrupt flag
;      portd &= 0b11111100;     // turn RD0 & RD1 outputs off
;      t1con.TMR1ON = 0;        // turn TMR1 off
;      channel ^= 3;            // toggle channel select bits
;    }
;  }
;
ISR
        banksel PIR1            ; bank 0                                  |B0
T2Proc
        btfss   PIR1,TMR2IF     ; TMR2 interrupt? yes, skip, else         |B0
        bra     T1Proc          ; branch                                  |B0
        bcf     PIR1,TMR2IF     ; clear TMR2 interrupt flag               |B0
        movf    PORTD,W         ;                                         |B0
        iorwf   Channel,W       ; Channel = 00000010 or 00000001          |B0
        movwf   PORTD           ; output new RD0 (Y) or RD1 (X) pulse     |B0
        movf    XHOLD_L,W       ;                                         |B0
        btfsc   PORTD,0         ;                                         |B0
        movf    YHOLD_L,W       ;                                         |B0
        movwf   TMR1L           ;                                         |B0
        movf    XHOLD_H,W       ;                                         |B0
        btfsc   PORTD,0         ;                                         |B0
        movf    YHOLD_H,W       ;                                         |B0
        movwf   TMR1H           ;                                         |B0
        bsf     T1CON,TMR1ON    ;                                         |B0
        retfie                  ;                                         |B0
T1Proc
        bcf     PIR1,TMR1IF     ; clear TMR1 interrupt flag               |B0
        movf    PORTD,W         ;                                         |B0
        andlw   b'11111100'     ;                                         |B0
        movwf   PORTD           ; turn RD0 & RD1 outputs off              |B0
        bcf     T1CON,TMR1ON    ; turn timer 1 off                        |B0
        movlw   3               ;                                         |B0
        xorwf   Channel,F       ; Channel = 00000001 or 00000010          |B0
        retfie                  ;                                         |B0
;

Thanks for your code, I'll examine it and see if it is a good fit. At first glance, it does seem to be a good bit tighter. I'm not using consecutive pins because I had the first servo on RC1, adding to the next consecutive pin messed it up. I tried for over an hour to figure it out, but I found that RC0 was staying high no matter what I did. I tried and tried and tried, and just gave in and switched to another port entirely to be done with it. I had even thought about posting on the microchip forum and asking if it was a limitation or flaw of/in the device.

As to the timer interrupts:
Some other guys, perhaps you, had given me some help on creating the period in another thread and I kind of had an "ah ha" moment from that. I didn't fully think through the stopping of timer2 thing before I started timer1, but after dedicating about 30 seconds of reasoning to it I had determined that the frames might get off if I didn't. What I didn't want to happen was to be using timer1 for say pulse generation on the X axis servo, and then a timer2 interrupt come along and try and reload tmr1 for the Y axis servo pulse before it was finished on the X axis servo. After thinking about it a bit more, I believe you are right and I could keep it running while I'm running tmr1.

I also like this method because it really allows you to drive quite a few servos with minimal program interference. My very rough (as in just counting instruction cycles in the ISR and multiplying that by the minimum frame needed for controlling two servos) calculation shows that the program is in the ISR less that 0.1% of the time at 32mhz. If you wanted to control say, 10 servos, you could just chop down the timer2 interrupt period and according to the same crude calculations, and assuming the ISR lengthens to 175 instruction cycles, you still spend just shy of 1% of the time in the ISR.

 

[skyhawk]

... and X and Y are both perpendicular to the direction of gravity at 0 g.

True at zero tilt, but I understand that you want to measure tilt. Image that you have a 3-axis accelerometer. Let the x-axis point forward, the y-axis to the right, and the z-axis down. If the device isn't accelerating and is level then the x and y-axis accelerometers read zero and the z-axis accelerometer reads 1 g. Now tilt the device in pitch (rotation around the y-axis). Now the x accelerometer reads g sinθ the z accelerometer reads g cosθ, and the y accelerometer still reads zero. By the usual trig identity the square root of the sum of squares of the x and z readings gives g. Dividing the x reading by g gives sinθ. For small angles Sinθ is approximately equal to θ in radians. If you don't know already, you multiply by π/180 to convert from degrees to radians.

Example:

θ(deg) θ(rad) sin θ
10.0 0.175 0.174
20.0 0.350 0.342
30.0 0.524 0.500

Similarly for roll (rotation around the x-axis). Simultaneous roll and pitch is a bit more complicated, but easily doable once the basic idea is understood.

 

[Mr RB]

WannaBinventor-

Also, there were only 20ish measurements in my arc cosine lookup table. As you guys probably know, as acos approaches 1 and -1, it's slope steepens quickly. With only tenths place accuracy, the first step is about 26 degrees! Now using hundreths place accuracy, it is still a big jump from 0 (8 degrees), but it flattens pretty quickly.

The ADC should only ever be relied on to be within +/- 1 count accuracy and stability on any one ADC reading.

With your ADC resolution a *single* ADC count might be a change in sensed angle of 10 or 26 degrees!! I think it is fundametally flawed using a sensor that will have an error of +/- 10 degrees or more on any ADC reading to try to control the stable position of a platform! You should increase your sensor resolution to better than 1 degree per ADC count, and use some software averaging to increase stability further.

 

[wannaBinventor]

You are right about there being a fundamental flaw there.

Right now, it's still using just an 8 bit result, but I'm taking a ten measurement rolling average. It has smoothed things out a good bit.

When some time frees up I'll probably change it all to work with all ten bits of the ADRES. I'd also like to go out to the thousandths with my g force calculations if I did that, but I'd need to get an actual arc cos (or arc sin as another suggested) routine working in assembly versus using a lookup table, which is already too long IMO (100 lines+).

I wonder if the dissassembly listing of a compiled C program using the acos function would give me something to work with on building the asm routine.