Dancebot not staying vertically

Karpovskaia

I have had some major progress with my dance bot. I am currently using the modified code of Hanno's dance bot by Duane. But I have an adjustment problem. When I first load the program the velT, tilt, and the turn move back and fourth and takes about 10 seconds before they center in the fuzzy tab in viewport. Once they do center and the robot is tilted slightly there is a small delay for the velT, tilt, and the turn to catch up with the robot as it tilts. Its like the accelerometer needs to speed up or the code needs to read the sensers faster. My newbe experience tells me that the waitcnt (nCnt+=clkfreq/200) ' update tilt every 5ms. Needs to be be adjusted, but it did not work. I also tried other things in the tilt code, but no luck. If there are any suggestions. That would be awesome.

Duane Degn

Hi Karpovskaia,

As I mentioned, I haven't made a balancing bot myself but I can think of several reasons why your bot could be having a hard time balancing.

My first suspicion is the control algorithm may not have a large enough proportional response to the sensor data. I think this would be part of "tuning" the various parameters of the robot's control algorithm.

My second suspicion is the motors aren't powerful or fast enough for the robot you've built.

Could you describe your setup? What motors are you using (preferably with a link)? What h-bridge circuit? What sensors are you using?

What kind of power supply? What kind and size of batteries?

It would also help if you attach an archive of your project so we could look at the code.

In order to archive your project, you need to first compile it. Then from the "File" menu select "Archive 'NameOfProject'" \ "Project...". This will create a zip file of all the files used in your project. You can attach the zip file by clicking the "Go Advanced" button (it on the bottom right of the text editor box when replying to a thread) and using the "Manage Attachment" feature (another button below the "advanced" text editor) to upload the file.

It would also be great if you could take a picture of your robot and attach it to your post.

For those who may be interested, crazyrobotgirl worked on a balancing bot here and here. I think Karpovskaia is using code from these threads.

Karpovskaia

When I load the program. I have to hold my robot upright for about 10 sec so the accelerometer can center itself.The sensors are moving from left to right. The motors will spin. But once the accelerometer finds its center. The motors stop spinning. I can then slowly tilt my robot in either direction and the motors will spin in the proper direction at the correct speed. As I tilt further the motors will speed up. The big problem is the accelerometer seems to be to slow for lack of better words. It doesn't center fast enough. I use the same motors, gyro, and type of battery like hanno. I used the same design frame like crazy robot girl. I found the same parts at Lowes.

Karpovskaia

How wuld I tune the sensers in the controll algorithm?

crazyrobotgirl

I am having the same problems as well. Duane has helped me alot
and Im trying to do the rest on my own. The whole PID thing is very complicated.

Karpovskaia

I modeled my bot after yours. I saw you're pics that you posted, and found the parts at Lowes.

crazyrobotgirl

Cool. Just keep at it. And if you figure it out, and get it working
let me know.

Duane Degn

Karpovskaia wrote: »

How wuld I tune the sensers in the controll algorithm?

I don't have the DanceBot code handy. If you post your archive, I can make sure I'm looking at the same code you're using.

I don't recall if Hanno used a full PID algorithm or not.

Wurlitzer

In my former life, I had to program numerous PID loops for a plethora of applications so I had no profile to start with. These could be mechanical (print registration at 3000'/minute), temperature, flow rates, etc.

The following always got me out of trouble.

Turn off all "I" and "D" gains run with proportional only.

Start with a low "P" gain and increase just until you get overshoot and the device, temperature, flow...whatever starts to oscillate around the desired set point. At this point back off the "P" gain until the oscillation stops. [NOTE: you will most likely NEVER be at the set point but close. This was usually the optimal setting for "P".

I would then enable the I gain and adjust till, once again, oscillation starts then back it off.

At this point, depending on the system, I usually did not need any differential (D) gain but if I did, I followed the same routine but this time making sure the system was neither too slow or fast to respond.

If you only work in one field you can evaluate the system and determine good starting points but in reality you may not know every variable so the method above will generate a tight PID loop control.

Karpovskaia

_Tilt.spin

I hope I did this right.

Karpovskaia

I was hopeing to be able to place the code without having you guys to have to download it. Wurlitzer by looking at the code can you give me an idea of what you are talking about ?

Duane Degn

Karpovskaia wrote: »

I hope I did this right.

I looks like you did attached the code correctly but the attached code is just the object for monitoring the gyro and accelerometer. I would like to see all the code. An archive of the top object will include all the objects used in the project. I explained how to archive your project in post #2.

With an archive, I can be sure I have the same version of child objects as used in your project.

Karpovskaia wrote: »

I was hopeing to be able to place the code without having you guys to have to download it.

It's usually better to post large amounts of code as an attachment. Small sections of code can be inserted in a post by using code tags. Phil wrote a tutorial of how to use code tags. (He also made the picture used to link to the tutorial.)

Karpovskaia

{***************************************************
* (C) 2010 HannoWare.com                           *
* Make the DanceBot dance with vision!             *
* AppletImage=motor.gif                            *
****************************************************}
[/COLOR][/SIZE][/COLOR][/SIZE][B][SIZE=3]CON
 _clkmode        
[/SIZE][/B][SIZE=3]= [B]xtal1 [/B]+ [B]pll16x[/B]
[B] _xinfreq        
[/B]= 
[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]5_000_000[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b] 
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]#[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]0[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],RunUpdate,RunFull  [/SIZE]
[SIZE=3] 
[/SIZE][SIZE=3][COLOR=#a89b38][SIZE=3][COLOR=#a89b38]'Fuzzy Logic Constants [/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#a89b38][SIZE=3][COLOR=#a89b38] 
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]#[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]0[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],FposE,FvelT,FvelE,FtiltT,FtiltE,Fturn,Fmotor [/SIZE][SIZE=3][COLOR=#a89b38][SIZE=3][COLOR=#a89b38]'fuzzy maps[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#a89b38][SIZE=3][COLOR=#a89b38] 
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]#[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]0[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],Rtilt,Rmotor                                 [/SIZE][SIZE=3][COLOR=#a89b38][SIZE=3][COLOR=#a89b38]'fuzzy rules[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#a89b38][SIZE=3][COLOR=#a89b38]  
  
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]maxti=[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]100000                  [/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3][COLOR=#a89b38][SIZE=3][COLOR=#a89b38]'maximum tilt before it bot has fallen[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#a89b38][SIZE=3][COLOR=#a89b38][/COLOR][/SIZE][/COLOR][/SIZE][B][SIZE=3]OBJ
  
[/SIZE][/B][SIZE=3]f:     
[/SIZE][B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333]"fuzzy.spin"           [/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3][COLOR=#a73737][SIZE=3][COLOR=#a73737]'fuzzy logic engine[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#a73737][SIZE=3][COLOR=#a73737]  
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]m:     [/SIZE][B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333]"_Motor"               [/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3][COLOR=#a73737][SIZE=3][COLOR=#a73737]'motor control [/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#a73737][SIZE=3][COLOR=#a73737]  
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]t:     [/SIZE][B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333]"_Tilt"                [/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3][COLOR=#a73737][SIZE=3][COLOR=#a73737]'read tilt of bot[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#a73737][SIZE=3][COLOR=#a73737]  
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]vp :   [/SIZE][B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333]"Conduit"              [/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3][COLOR=#a73737][SIZE=3][COLOR=#a73737]'debug with ViewPort[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#a73737][SIZE=3][COLOR=#a73737]  
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]qs:    [/SIZE][B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333]"QuickSample"          [/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3][COLOR=#a73737][SIZE=3][COLOR=#a73737]'monitor IO port[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#a73737][SIZE=3][COLOR=#a73737]  
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]freq: [/SIZE][B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333]"synth"                 [/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3][COLOR=#a73737][SIZE=3][COLOR=#a73737]'output clock to ADC[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#a73737][SIZE=3][COLOR=#a73737]  
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]video: [/SIZE][B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333]"PropCVCapture"        [/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3][COLOR=#a73737][SIZE=3][COLOR=#a73737]'capture video signal and view in Viewport[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#a73737][SIZE=3][COLOR=#a73737]  
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]ve:    [/SIZE][B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333]"PropCVFilter"         [/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3][COLOR=#a73737][SIZE=3][COLOR=#a73737]'applies vision algorithms to video  [/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#a73737][SIZE=3][COLOR=#a73737][/COLOR][/SIZE][/COLOR][/SIZE][B][SIZE=3]var
  long 
[/SIZE][/B][SIZE=3]io,pos,tangle,tvel,trot,rot,rotpos,vel,tilt,turn,motor,posT,velT,tiltT,posE,velE,tiltOff,tiltE,tiltI,bb,bx,by,fuzz[
[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]260[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]][/SIZE]
[SIZE=3]  
[/SIZE][SIZE=3][COLOR=#a76f37][SIZE=3][COLOR=#a76f37]{lots of interesting variables to follow with viewport:[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#a76f37][SIZE=3][COLOR=#a76f37]   io: status of io port
   pos: position of robot as measured by encoder
   tangle: target angle of tilt for bot
   tvel: target velocity 
   trot: amount bot should rotate left/right
   rot: amount of differential steer bot is using now
   rotpos: not used, was for integrating rot
   vel: current velocity
   tilt: current tilt
   turn: current turn amount
   motor: current speed of one motor
   posT: target position
   velT: target velocity
   tiltT: target tilt
   posE: position error
   velE: velocity error
   tiltOff: offset in tilt- calibrated so tilt reads 0 when balanced
   tiltE: tilt error
   tiltI: integrated tilt
   bx:x location of blob
   by:y location of blob
   fuzz: structure for fuzzy logic} 
  
[/COLOR][/SIZE][/COLOR][/SIZE][B][SIZE=3]long [/SIZE][/B][SIZE=3]mode,ncnt,oldPos[[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]10[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]],avgPos[/SIZE]
[SIZE=3]  
[/SIZE][SIZE=3][COLOR=#a76f37][SIZE=3][COLOR=#a76f37]{mode: bot can be updating sensor, or running full- with motor control[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#a76f37][SIZE=3][COLOR=#a76f37]   ncnt: next cnt- used to control loop time
   oldPos: average the bot's position
   avgPos: result of averaging}   
  
[/COLOR][/SIZE][/COLOR][/SIZE][B][SIZE=3]long [/SIZE][/B][SIZE=3]videoFrame[[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]3000[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]] [/SIZE][SIZE=3][COLOR=#a76f37][SIZE=3][COLOR=#a76f37]'data for captured video[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#a76f37][SIZE=3][COLOR=#a76f37]  
[/COLOR][/SIZE][/COLOR][/SIZE][B][SIZE=3]long [/SIZE][/B][SIZE=3]rawsee [/SIZE][SIZE=3][COLOR=#a76f37][SIZE=3][COLOR=#a76f37]'raw value from propfilter, contains x,y of blob[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#a76f37][SIZE=3][COLOR=#a76f37][/COLOR][/SIZE][/COLOR][/SIZE][B][SIZE=3]pub 
[/SIZE][/B][SIZE=3]main|frame[400]
[/SIZE][SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]'main loop: start subsystems, then drive bot using vision
  
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]vp.register(qs.sampleINA(@frame,[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]1[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3])) [/SIZE][SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]'sample INA with 1 cog up to 20Mhz[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]  
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]start1[/SIZE]
[SIZE=3]  vp.register(f.start(@fuzz))
  vp.register(video.start(@videoFrame,video#VIDEO2))  
[/SIZE][SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]'1 vidoe buffer  [/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]  
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]vp.config(string([/SIZE][B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333]"var:io(bits=[6acCL,7acDT,5gyrOT,8gyrIn,4m1p,3m1r,27m2p,26m2r,12encA,13encB]),pos,tangle,tvel,trot,rot,rotpos,vel,tilt,turn,motor,Speed,velT,tiltT,posE,velE,tiltOff,tiltE,tiltI,bb,bx,by"[/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]))[/SIZE]
[SIZE=3]  vp.config(string(
[/SIZE][B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333]"lsa:view=io,timescale=1ms"[/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]))[/SIZE]
[SIZE=3]  vp.config(string(
[/SIZE][B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333]"video:view=bx"[/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]))   [/SIZE]
[SIZE=3]  vp.config(string(
[/SIZE][B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333]"dso:view=[bx,by],timescale=1s"[/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]))[/SIZE]
[SIZE=3]  vp.config(string(
[/SIZE][B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333]"start:dso"[/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]))[/SIZE]
[SIZE=3]  vp.share(@pos,@by)
  Freq.Synth(
[/SIZE][B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333]"B"[/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]24[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3], [/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]10_000_000[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]) [/SIZE][SIZE=3][COLOR=#326369][SIZE=3][COLOR=#326369]''generate signal for the ADC[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#326369][SIZE=3][COLOR=#326369]  
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]start2[/SIZE]
[SIZE=3]  video.start(@rawsee,video#BLOB)
  tvel~
[/SIZE][SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]'-2[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]  
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]trot~[/SIZE]
[SIZE=3]  
[/SIZE][B][SIZE=3][COLOR=#808080][SIZE=3][COLOR=#808080]repeat[/COLOR][/SIZE][/COLOR][/SIZE][/B]
[B][SIZE=3][COLOR=#808080][SIZE=3][COLOR=#808080]    
[/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#808080][SIZE=3][COLOR=#808080][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]do(1,tvel,trot)[/SIZE]
[SIZE=3]    bx:=rawsee 
[/SIZE][B][SIZE=3][COLOR=#008000][SIZE=3][COLOR=#008000]& [/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#008000][SIZE=3][COLOR=#008000][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]$ff[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]    
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]by:=(rawsee>>8) [/SIZE][B][SIZE=3][COLOR=#008000][SIZE=3][COLOR=#008000]& [/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#008000][SIZE=3][COLOR=#008000][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]$ff[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]    
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]tvel+=by[/SIZE]
[SIZE=3]    trot+=bx
[B]pub 
[/B]restart
  
[/SIZE][SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]'when bot falls, restart with fresh position[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]  
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]mode:=runFull[/SIZE]
[SIZE=3]  pos~
  posT~
  rotpos~
  longfill(@oldPos,
[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]0[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]10[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3])[/SIZE]
[SIZE=3][B]pub 
[/B]start2
  
[/SIZE][SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]'start motor and tilt cogs[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]  
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]mode:=runUpdate[/SIZE]
[SIZE=3]  tiltOff:=-360_000
  m.start(@pos)
  t.start
  ncnt:=cnt+8000_000
[B]pub 
[/B]start1 
  
[/SIZE][SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]'initialize fuzzy logic maps[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]  
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]f.setVar(@pos)[/SIZE]
[SIZE=3]  f.setMap(FposE,@posE,
[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]3000[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]10000[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]) [/SIZE]
[SIZE=3]  f.setMap(FvelT,@velT,
[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]10[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]30[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3])[/SIZE]
[SIZE=3]  f.setMap(FvelE,@velE,
[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]800[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]1600[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3])[/SIZE]
[SIZE=3]  f.setMap(FtiltT,@tiltT,
[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]30_000[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]100_000[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]) [/SIZE]
[SIZE=3]  f.setMap(FtiltE,@tiltE,
[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]40_000[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]120_000[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3])[/SIZE]
[SIZE=3]  f.setMap(Fturn,@turn,
[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]40_000[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]90_000[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3])    [/SIZE]
[SIZE=3]  f.setMap(Fmotor,@motor,
[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]2000[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]7000[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3])[/SIZE]
[SIZE=3]  f.setRule(Rtilt,@posE,@velE,@Mtilt)
  f.setRule(Rmotor,@tiltE,@turn,@Mmotor)
  f.start(@fuzz)  
[B]pub 
[/B]fuzzyControl |t1,t2
  
[/SIZE][SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]'cascading fuzzy logic pid controller[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]    
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]posE:=-pos+posT            [/SIZE][SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]'posE=positive if on right of target           [/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]    
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]f.fuzzifyA2(posE,FposE)    [/SIZE][SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]'A=fuzzy posE[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]    
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]velT:=-f.defuzzify2(FvelT)  [/SIZE][SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]'velT=positive if need to move right[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]    
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]t1:=f.defuzzify2(FtiltT)[/SIZE]
[SIZE=3]    velE:=vel-velT            
[/SIZE][SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]'velE=positive if going slower than target[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]    
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]f.fuzzifyA2(velE,FvelE)    [/SIZE][SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]'A=fuzzy velE[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]    
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]t2:=f.defuzzify2(FtiltT)[/SIZE]
[SIZE=3]    
[/SIZE][SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]'-t1-t2 is a pi controller, need d which is acceleration, use tilt[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]    
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]tiltT:=-t1-t2-tilt/6[/SIZE]
[SIZE=3]    f.share(@fuzz)
    tiltE:=tilt-tiltT         
[/SIZE][SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]'tiltE=positive if tilted right of target[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]    
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]tiltI+=tiltE/200              [/SIZE][SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]'tiltI=positive if tilted right for too long[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]    
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]tiltI:= (tiltI<# maxTI) [/SIZE][B][SIZE=3][COLOR=#008000][SIZE=3][COLOR=#008000]#> [/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#008000][SIZE=3][COLOR=#008000][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]-maxTI  [/SIZE]
[SIZE=3]    f.fuzzifyA2(tiltE,FtiltE)  
[/SIZE][SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]'A=fuzzy tiltE[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]    
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]t1:=f.defuzzify2(Fmotor)[/SIZE]
[SIZE=3]    f.fuzzifyA2(turn,Fturn)    
[/SIZE][SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]'A=fuzzy turn=positive if turning right[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]    
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]t2:=f.defuzzify2(Fmotor)[/SIZE]
[SIZE=3]    motor:=-t1-t2/2
[/SIZE][SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]'+tiltI/1500[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7][/COLOR][/SIZE][/COLOR][/SIZE][B][SIZE=3]pub 
[/SIZE][/B][SIZE=3]balance|mpos
  
[/SIZE][SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]'update readings, then balance using fuzzy logic[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]    
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]t.update1(80_000) [/SIZE]
[SIZE=3]    
[/SIZE][B][SIZE=3][COLOR=#808080][SIZE=3][COLOR=#808080]if [/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#808080][SIZE=3][COLOR=#808080][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]||(tiltE)> [/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]500_000 [/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]'if can't match tilt, then stop motors[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]      
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]mode:=runUpdate[/SIZE]
[SIZE=3]    mPos:=pos
[/SIZE][SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]'-rotPos 'read in pos for this round, adjust for turns[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]    
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]vel:=(mPos-oldPos[avgPos])[/SIZE]
[SIZE=3]    oldPos[avgPos]:=mPos
    avgPos+=1  
[/SIZE][SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]'averages velocity by subtracting current position to position(t-10)[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]    
[/COLOR][/SIZE][/COLOR][/SIZE][B][SIZE=3][COLOR=#808080][SIZE=3][COLOR=#808080]if [/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#808080][SIZE=3][COLOR=#808080][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]avgPos [/SIZE][B][SIZE=3][COLOR=#008000][SIZE=3][COLOR=#008000]> [/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#008000][SIZE=3][COLOR=#008000][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]9[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]      
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]avgPos~[/SIZE]
[SIZE=3]    tilt:=t.update2(@turn)-tiltOff
    fuzzyControl
    
[/SIZE][B][SIZE=3][COLOR=#808080][SIZE=3][COLOR=#808080]case [/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#808080][SIZE=3][COLOR=#808080][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]mode[/SIZE]
[SIZE=3]      runUpdate:
        m.update(0,
[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]0[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3])[/SIZE]
[SIZE=3]        
[/SIZE][B][SIZE=3][COLOR=#808080][SIZE=3][COLOR=#808080]if [/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#808080][SIZE=3][COLOR=#808080][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]||(tiltE)<40_000[/SIZE]
[SIZE=3]          restart
      runFull:
        m.update(motor-rot,motor+rot)
        m.update(-motor+rot,-motor-rot)
    waitcnt(nCnt+=400000) 
[/SIZE][SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]'wait 5msec[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]    
[/COLOR][/SIZE][/COLOR][/SIZE][B][SIZE=3]return [/SIZE][/B][SIZE=3]mode [/SIZE][B][SIZE=3][COLOR=#008000][SIZE=3][COLOR=#008000]<> [/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#008000][SIZE=3][COLOR=#008000][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]runFull    [/SIZE]
[SIZE=3][B]pub 
[/B]do(n,_vel,_rot) 
  
[/SIZE][SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]'stays in balance while moving with vel, rotating trot[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]  
[/COLOR][/SIZE][/COLOR][/SIZE][B][SIZE=3][COLOR=#808080][SIZE=3][COLOR=#808080]repeat [/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#808080][SIZE=3][COLOR=#808080][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]n[/SIZE]
[SIZE=3]    posT+=_vel
    rot:=_rot
    
[/SIZE][B][SIZE=3][COLOR=#808080][SIZE=3][COLOR=#808080]if [/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#808080][SIZE=3][COLOR=#808080][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]tangle>15000[/SIZE]
[SIZE=3]      tangle-=15000
    
[/SIZE][B][SIZE=3][COLOR=#808080][SIZE=3][COLOR=#808080]elseif [/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#808080][SIZE=3][COLOR=#808080][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]tangle<0[/SIZE]
[SIZE=3]      tangle+=15000
    
[/SIZE][B][SIZE=3][COLOR=#808080][SIZE=3][COLOR=#808080]if [/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#808080][SIZE=3][COLOR=#808080][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]balance[/SIZE]
[SIZE=3]      [B]return false    [/B]
[B]  return true
dat 
[/B][/SIZE][SIZE=3][COLOR=#37a747][SIZE=3][COLOR=#37a747]'Fuzzy Logic Maps[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#37a747][SIZE=3][COLOR=#37a747]  
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]mtilt [B]byte [/B][/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]0[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]0[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]0[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]1[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]2[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]        
[/COLOR][/SIZE][/COLOR][/SIZE][B][SIZE=3]byte [/SIZE][/B][SIZE=3][/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]0[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]0[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]1[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]2[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]3[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]        
[/COLOR][/SIZE][/COLOR][/SIZE][B][SIZE=3]byte [/SIZE][/B][SIZE=3][/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]0[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]1[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]2[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]3[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]4[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]        
[/COLOR][/SIZE][/COLOR][/SIZE][B][SIZE=3]byte [/SIZE][/B][SIZE=3][/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]1[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]2[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]3[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]4[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]4[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]        
[/COLOR][/SIZE][/COLOR][/SIZE][B][SIZE=3]byte [/SIZE][/B][SIZE=3][/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]2[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]3[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]4[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]4[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]4[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],  [/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]0[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]0[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]0 [/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3][COLOR=#37a747][SIZE=3][COLOR=#37a747]'last 3 are needed to make 28 bytes=7 longs        [/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#37a747][SIZE=3][COLOR=#37a747] 
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]mmotor [B]byte [/B][/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]0[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]0[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]0[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]1[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]2[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]        
[/COLOR][/SIZE][/COLOR][/SIZE][B][SIZE=3]byte [/SIZE][/B][SIZE=3][/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]0[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]0[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]1[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]2[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]3[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]        
[/COLOR][/SIZE][/COLOR][/SIZE][B][SIZE=3]byte [/SIZE][/B][SIZE=3][/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]0[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]1[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]2[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]3[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]4[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]        
[/COLOR][/SIZE][/COLOR][/SIZE][B][SIZE=3]byte [/SIZE][/B][SIZE=3][/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]1[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]2[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]3[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]4[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]4[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]        
[/COLOR][/SIZE][/COLOR][/SIZE][B][SIZE=3]byte [/SIZE][/B][SIZE=3][/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]2[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]3[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]4[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]4[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]4[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],  [/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]0[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]0[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]0 [/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3][COLOR=#37a747][SIZE=3][COLOR=#37a747]'last 3 are needed to make 28 bytes=7 longs  [/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#37a747][SIZE=3][COLOR=#37a747]        

Karpovskaia

The first was the parent object.
This one is the child object.

[/SIZE][SIZE=3][COLOR=#a89b38][SIZE=3][COLOR=#a89b38]{*******************************************************
* (C) 2008 myDanceBot.com, Inc.                        *
********************************************************}
[/COLOR][/SIZE][/COLOR][/SIZE][B][SIZE=3]CON 
[/SIZE][/B][SIZE=3][/SIZE][SIZE=3][COLOR=#a89b38][SIZE=3][COLOR=#a89b38]' replaces original "con" section[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#a89b38][SIZE=3][COLOR=#a89b38]  
[/COLOR][/SIZE][/COLOR][/SIZE][B][SIZE=3]_clkmode [/SIZE][/B][SIZE=3]= [B]xtal1 [/B]+ [B]pll16x[/B][/SIZE]
[SIZE=3][B]  _xinfreq 
[/B]= 
[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]5_000_000[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]  
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]msec = [/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]80_000[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]  
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]biasUpdate= [/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]80_000_000[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]/[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]50 [/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3][COLOR=#a89b38][SIZE=3][COLOR=#a89b38]'20ms[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#a89b38][SIZE=3][COLOR=#a89b38]  
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]gAvgNum = [/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]3 [/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3][COLOR=#a89b38][SIZE=3][COLOR=#a89b38]'shifted in code, so change that too[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#a89b38][SIZE=3][COLOR=#a89b38]  
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]MEMSIC_PIN_X = [/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]6[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]  
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]MEMSIC_PIN_Y = [/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]7[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]  
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]gyroOutP = [/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]8[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]  
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]gyroInP  = [/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]5[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]  
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]gyroS    = [/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]40[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]  
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]gyroO    = [/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]82535[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]  
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]ZERO_G_MEMSIC = [/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]400_000 [/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3][COLOR=#a89b38][SIZE=3][COLOR=#a89b38]' clock cycles[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#a89b38][SIZE=3][COLOR=#a89b38]                          ' This may need to be adjusted to match your accelerometer.
                          
  
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]CONVERSION_FACTOR = ([/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]2047 [/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]* [/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]800[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]) / (ZERO_G_MEMSIC / [/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]3[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3])[/SIZE]
[SIZE=3]                
[/SIZE][SIZE=3][COLOR=#a89b38][SIZE=3][COLOR=#a89b38]' assumes one G on original accelerometer[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#a89b38][SIZE=3][COLOR=#a89b38]                ' returns "2047" and the pulse on the Memsic
                ' varies by 400_000 at over three Gs (divide by three
                ' to get one G).
[/COLOR][/SIZE][/COLOR][/SIZE][B][SIZE=3]VAR
  long 
[/SIZE][/B][SIZE=3]gyrodone
  [B]long [/B]accel,Egyro,Etilt,bias,gyroInt,p[
[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]4[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]],kp[[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]4[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]] [/SIZE][SIZE=3][COLOR=#a76f37][SIZE=3][COLOR=#a76f37]'interesting  [/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#a76f37][SIZE=3][COLOR=#a76f37]  
[/COLOR][/SIZE][/COLOR][/SIZE][B][SIZE=3]long [/SIZE][/B][SIZE=3]frame[[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]406[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]][/SIZE]
[SIZE=3]  [B]long [/B]gyro,tilt
  [B]long [/B]biasF,QgyroF,QaccelF,RaccelF,dtF,dt,Qgyro,Qaccel,Raccel,f1,f100k 
[/SIZE][SIZE=3][COLOR=#a76f37][SIZE=3][COLOR=#a76f37]'kalman[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#a76f37][SIZE=3][COLOR=#a76f37]  
[/COLOR][/SIZE][/COLOR][/SIZE][B][SIZE=3]long [/SIZE][/B][SIZE=3]stack[[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]85[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]] [/SIZE][SIZE=3][COLOR=#a76f37][SIZE=3][COLOR=#a76f37]'55 needed[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#a76f37][SIZE=3][COLOR=#a76f37]  
[/COLOR][/SIZE][/COLOR][/SIZE][B][SIZE=3]long [/SIZE][/B][SIZE=3]gAvg[gAvgNum+[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]1[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]][/SIZE]
[SIZE=3]                
[/SIZE][SIZE=3][COLOR=#a76f37][SIZE=3][COLOR=#a76f37]' The "800" is included from Hanno's code.[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#a76f37][SIZE=3][COLOR=#a76f37][/COLOR][/SIZE][/COLOR][/SIZE][B][SIZE=3]OBJ 
[/SIZE][/B][SIZE=3][/SIZE][SIZE=3][COLOR=#a73737][SIZE=3][COLOR=#a73737]' replaces original "obj section"[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#a73737][SIZE=3][COLOR=#a73737]  
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]Memsic : [/SIZE][B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333]"Memsic2125_v1.2"[/COLOR][/SIZE][/COLOR][/SIZE][/B]
[B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333]  
[/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]m : [/SIZE][B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333]"FloatMath"[/COLOR][/SIZE][/COLOR][/SIZE][/B]
[B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333]  
[/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]vp : [/SIZE][B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333]"Conduit"[/COLOR][/SIZE][/COLOR][/SIZE][/B]
[B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333]  
[/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]qs : [/SIZE][B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333]"QuickSample"[/COLOR][/SIZE][/COLOR][/SIZE][/B]
[B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333]　
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]pub 
[/SIZE][/B][SIZE=3]main|nCnt,stack1[160]
  vp.register(qs.sampleINA(@frame,
[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]1[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3])) [/SIZE][SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]'sample INA with 1 cog up to 20Mhz[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]  
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]vp.config(string([/SIZE][B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333]"var:io,accel,gyro,tilt,bias,gyroInt(acdc=ac),p1,p2,p3,p4,k1,k2,k3,k4"[/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]))  [/SIZE]
[SIZE=3]  vp.config(string(
[/SIZE][B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333]"lsa:view=io,timescale=1ms,trigger=io[6]r"[/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3])) [/SIZE]
[SIZE=3]  vp.config(string(
[/SIZE][B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333]"dso:view=[accel,tilt],timescale=1s"[/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]))[/SIZE]
[SIZE=3]  vp.config(string(
[/SIZE][B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333]"start:dso"[/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#f73333][SIZE=3][COLOR=#f73333][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]))[/SIZE]
[SIZE=3]  vp.share(@accel,@gyroInt+8*4)
  waitcnt(cnt+clkfreq*1)
  start
  ncnt:=cnt
  
[/SIZE][B][SIZE=3][COLOR=#808080][SIZE=3][COLOR=#808080]repeat[/COLOR][/SIZE][/COLOR][/SIZE][/B]
[B][SIZE=3][COLOR=#808080][SIZE=3][COLOR=#808080]    
[/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#808080][SIZE=3][COLOR=#808080][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]update1(80_000)[/SIZE]
[SIZE=3]    Etilt:=update2(@Egyro)    
    waitcnt(nCnt+=clkfreq/200) 
[/SIZE][SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]'update tilt every 5ms}}   [/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7][/COLOR][/SIZE][/COLOR][/SIZE][B][SIZE=3]pub 
[/SIZE][/B][SIZE=3]start|a,b,c,d
  longfill(@accel,
[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]9[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3],[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]0[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3])[/SIZE]
[SIZE=3]  longfill(@gavg,gAvgNum,
[/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]0[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3])[/SIZE]
[SIZE=3]  dira[gyroOutP]~~
  [B]ctra [/B]
[/SIZE][B][SIZE=3][COLOR=#008000][SIZE=3][COLOR=#008000]:= [/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#008000][SIZE=3][COLOR=#008000][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b](%10101 [/COLOR][/SIZE][/COLOR][/SIZE][B][SIZE=3][COLOR=#008000][SIZE=3][COLOR=#008000]<< [/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#008000][SIZE=3][COLOR=#008000][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]26 [/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]) [/SIZE][B][SIZE=3][COLOR=#008000][SIZE=3][COLOR=#008000]| [/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#008000][SIZE=3][COLOR=#008000][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b](%001 [/COLOR][/SIZE][/COLOR][/SIZE][B][SIZE=3][COLOR=#008000][SIZE=3][COLOR=#008000]<< [/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#008000][SIZE=3][COLOR=#008000][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]23[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]) [/SIZE][B][SIZE=3][COLOR=#008000][SIZE=3][COLOR=#008000]| [/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#008000][SIZE=3][COLOR=#008000][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]GyroInP [/SIZE][SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]' set up counter for gyroin, measured lag till pulseout[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]  
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]frqa:=2[/SIZE]
[SIZE=3]  bias~
  
[/SIZE][SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]'average gyro[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]  
[/COLOR][/SIZE][/COLOR][/SIZE][B][SIZE=3][COLOR=#808080][SIZE=3][COLOR=#808080]repeat [/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#808080][SIZE=3][COLOR=#808080][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]10  [/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#00008b][SIZE=3][COLOR=#00008b]    
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]update1(80_000)[/SIZE]
[SIZE=3]    waitcnt(GyroDone) 
    bias+= -(phsa-gyroO)
    outa[GyroOutP]~
    waitcnt(cnt+clkfreq/200) 
[/SIZE][SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]'update tilt every 5ms}}  [/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]  
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]bias/=10    [/SIZE]
[SIZE=3]  cognew(updateBias,@stack)
  
[/SIZE][B][SIZE=3][COLOR=#808080][SIZE=3][COLOR=#808080]repeat while [/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#808080][SIZE=3][COLOR=#808080][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]accel==0 [/SIZE][SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]'wait till accel read in[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]  
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]tilt:=accel[/SIZE]
[SIZE=3][B]pub 
[/B]update1(gyroTime)
  outa[GyroOutP]~~
  phsa~
  GyroDone:=cnt+gyroTime
[B]pub 
[/B]update2(gyroPtr)|a
  waitcnt(GyroDone) 
  gyro:= (phsa-gyroO)
  gyroInt+=gyro
  outa[GyroOutP]~
  a:=gyro-bias
  tilt+=a 
[/SIZE][SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]'bias changed by accel/kalman every 20ms[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]  
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]gavg[gAvgNum]:=a    [/SIZE]
[SIZE=3]  longmove(@gavg,@gavg+4,gAvgNum) 
[/SIZE][SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]'shift left[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]  
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]long[gyroPtr]:=a[/SIZE][SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]'+=a-gavg[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]  
[/COLOR][/SIZE][/COLOR][/SIZE][B][SIZE=3]return [/SIZE][/B][SIZE=3]tilt[/SIZE]
[SIZE=3][B]PUB 
[/B]updateBias 
[/SIZE][B][SIZE=3][COLOR=#008000][SIZE=3][COLOR=#008000]| [/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#008000][SIZE=3][COLOR=#008000][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]pulseFromMemsic, ncnt [/SIZE][SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]' replaces original updateBias method[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]  
  
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]StartKalman      [/SIZE]
[SIZE=3]  Memsic.start(MEMSIC_PIN_X, MEMSIC_PIN_Y)      
[/SIZE][SIZE=3][COLOR=#326369][SIZE=3][COLOR=#326369]'' Initialize Memsic2125 starts a cog[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#326369][SIZE=3][COLOR=#326369]  
  
[/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]ncnt:=cnt[/SIZE]
[SIZE=3]  
[/SIZE][B][SIZE=3][COLOR=#808080][SIZE=3][COLOR=#808080]repeat[/COLOR][/SIZE][/COLOR][/SIZE][/B]
[B][SIZE=3][COLOR=#808080][SIZE=3][COLOR=#808080]    
[/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#808080][SIZE=3][COLOR=#808080][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]pulseFromMemsic [/SIZE][B][SIZE=3][COLOR=#008000][SIZE=3][COLOR=#008000]:= [/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#008000][SIZE=3][COLOR=#008000][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]Memsic.Mx [/SIZE]
[SIZE=3]    accel:= (pulseFromMemsic 
[/SIZE][B][SIZE=3][COLOR=#008000][SIZE=3][COLOR=#008000]- [/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#008000][SIZE=3][COLOR=#008000][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]ZERO_G_MEMSIC) [/SIZE][B][SIZE=3][COLOR=#008000][SIZE=3][COLOR=#008000]* [/COLOR][/SIZE][/COLOR][/SIZE][/B][SIZE=3][COLOR=#008000][SIZE=3][COLOR=#008000][/COLOR][/SIZE][/COLOR][/SIZE][SIZE=3]CONVERSION_FACTOR[/SIZE]
[SIZE=3]    doKalman
    waitcnt(ncnt+=biasUpdate) 
[/SIZE][SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7]'20ms[/COLOR][/SIZE][/COLOR][/SIZE]
[SIZE=3][COLOR=#379ba7][SIZE=3][COLOR=#379ba7][/COLOR][/SIZE][/COLOR][/SIZE][B][SIZE=3]pub 
[/SIZE][/B][SIZE=3]StartKalman
  biasF:=m.ffloat(bias)
  dt:=50
  Qaccel:=1
  Qgyro:=0
  Raccel:=10000
  QgyroF:=m.fdiv(m.ffloat(Qgyro),m.ffloat(1))
  QaccelF:=m.fdiv(m.ffloat(Qaccel),m.ffloat(1))
  RaccelF:=m.fdiv(m.ffloat(RAccel),m.ffloat(1)) 
  dtF:=m.fdiv(m.ffloat(1),m.ffloat(dt))
  p[0]:=QgyroF
  p[1]:=m.fmul(QaccelF,dtF)
  p[2]:=m.fmul(QaccelF,dtF)
  p[3]:=QaccelF
  f1:=m.ffloat(1)
  f100k:=m.ffloat(100000)   
[B]pub 
[/B]doKalman|temp,kk[4],GyroF,AccelF,k1
  p[0]:=m.fadd(p[0],m.fadd(m.fmul(m.fneg(m.fadd(p[1],p[2])),dtF),QaccelF))
  temp:= m.fmul(m.fneg(p[3]),dtF)
  p[1]:=m.fadd(p[1],temp)
  p[2]:=m.fadd(p[2],temp)
  p[3]:=m.fadd(p[3],QgyroF)
  temp:=m.fdiv(f1,m.fadd(RaccelF,p[0]))
  kk[0]:=m.fmul(p[0],temp)
  kk[2]:=m.fmul(p[2],temp)
  temp:=m.fsub(m.ffloat(accel),m.ffloat(tilt))
  tilt+=m.fround(m.fmul(kk[0],temp))
  BiasF:=m.fadd(BiasF,m.fmul(kk[2],temp))
  p[3]:=m.fsub(p[3],m.fmul(kk[2],p[1]))
  p[2]:=m.fsub(p[2],m.fmul(kk[2],p[0]))
  p[1]:=m.fsub(p[1],m.fmul(kk[0],p[1]))
  p[0]:=m.fsub(p[0],m.fmul(kk[0],p[0]))
  bias:=m.fround(biasf)
  kp[0]:=m.fround(m.fmul(p[0],f100k))
  kp[1]:=m.fround(m.fmul(p[1],f100k))
  kp[2]:=m.fround(m.fmul(p[2],f100k))
  kp[3]:=m.fround(m.fmul(p[3],f100k))     

Duane Degn

An archive will include all the objects.

Load the top object in the Prop Tool and press F9 to compile it. Once the top object has been compiled you can create an archive.

Duane Degn wrote: »

In order to archive your project, you need to first compile it. Then from the "File" menu select "Archive 'NameOfProject'" \ "Project...". This will create a zip file of all the files used in your project.

The "Archive 'NameOfProject'" option will not actually have "NameOfProject" listed. The name of the top object of your project should be listed after "Archive".

I'm not positive which object the control algorithm is in. It's likely in one of the sections of code you posted but I would prefer to have a complete archive of the project before I look for it.

Wurlitzer

As I do not have the code nor need it, my post was to address tuning a PID loop (which I assumed was your original problem) in general REGARDLESS of the application. It has never failed me, but I have no idea where such control may lie in the program you are using.

If I were to explain PID to a person with NO knowledge of computers or control logic it would be, [and forgive me if I over simplify and this is GREATLY oversimplified] the following;

Proportional [P]: How far away from the ideal set point is my system? The further away, the more gain I would need to get to the set point. (Set Point - Actual) * P-gain

Integral : How long have I been away from the set point? If I am always away too long, increase the rate at which I add/subtract I-gain. Add I-gain every Y seconds. ( i have worked on systems where you can only set the amount of I-gain and the timed interval is fixed, or you can set both the I value and adjust the time between when I-gain is added or subtracted.) Either one works but being able to control both is more powerful IMO.

Derivative [D]: Is the combination of P & I moving the system too fast?

EG: Automobile cruise control: The desired speed (set Point) vs the Actual speed become my Error! So if I have a low P gain that [error * P] might never even bring me close to the right speed. If I have a lot of P gain I might over speed, then slow down, over speed, slow down....

Now lets say I add some gain every second I am under speed (or subtract ever second if I am going too fast), now P + I will get me closer to the set point faster but just as before if I add too much gain I will over shoot and the car will start to hunt for the right speed.

Now if I have set both [P] & as high as possible without overshooting my set point, I may or may not have to add [D] and usually I did not. There are some exceptions. In some applications you may be so far away from your target that your system will cause damage trying to reach the set point. IE: Car set point 60 MPH and actual speed 0. With a well tuned P & I we might lay down a long strip of burned rubber attempting to get to 60 MPH as fast as possible. [this is why cruise controls drop out usually below 30 MPH]. The [D] gain in applications like this will slow down the system response so you don't over stress individual components.

Hanno

Hi!
Wurlitzer- nice description of PID! There's certainly a lot of art/experimentation to tuning PID constants.
The need to tune PID constants and visualize how the control system was performing is what led me to develop ViewPort...

Duane- thanks for helping and inspire others with your "DanceBot" (and vision system).

Here are some words of wisdom for balancing bot newbies:
"The bot needs accurate and low-latency measurements of it's orientation. It must drive its wheel's with just the right amount of force. There shouldn't be any mechanical slop in the entire system."

If you're not doing all of the above, your bot won't balance as well as it should...
Hanno

Mickster

Duane Degn wrote: »

My first suspicion is the control algorithm may not have a large enough proportional response to the sensor data. I think this would be part of "tuning" the various parameters of the robot's control algorithm.

I would agree with this!

Not wishing to contradict Wurlitzer but the "inverted pendulum" nature of the DanceBot must present a significant reflected-inertia to the motor. I doubt that "PI" alone would ever stabilize such a load characteristic. In my (everyday) experience, keeping the "D" term at least an order of magnitude of the "P" term, allows me to increase the "P" term to provide the optimum response. Lastly, I include the "I" term with appropriate limiting to prevent windup.

Regards,

Mickster

Karpovskaia

DanceBot - Archive [Date 2013.08.19 Time 10.05].zip Ok here you go. I hope that this is right.
I think all the advices given to me above will go a long way with me and others. I know this is not a beginners project, but for me it is a platform. So thank you all.

crazyrobotgirl

Definitely good advice Wurlitzer on PID loop.

Mickster

In case it didn't click, the keyword in Hanno's post is "ViewPort". Highly recommended! :thumb:

Mickster

Duane Degn

I'm pretty sure the PID part of the control is taking place in the "fuzzycontrol" method.

pub fuzzyControl |t1,t2
  'cascading fuzzy logic pid controller
    posE:=-pos+posT            'posE=positive if on right of target           
    f.fuzzifyA2(posE,FposE)    'A=fuzzy posE
    velT:=-f.defuzzify2(FvelT)  'velT=positive if need to move right
    t1:=f.defuzzify2(FtiltT)
    velE:=vel-velT            'velE=positive if going slower than target
    f.fuzzifyA2(velE,FvelE)    'A=fuzzy velE
    t2:=f.defuzzify2(FtiltT)
    '-t1-t2 is a pi controller, need d which is acceleration, use tilt
    tiltT:=-t1-t2-tilt/6
    f.share(@fuzz)
    tiltE:=tilt-tiltT         'tiltE=positive if tilted right of target
    tiltI+=tiltE/200              'tiltI=positive if tilted right for too long
    tiltI:= (tiltI<# maxTI) #> -maxTI  
    f.fuzzifyA2(tiltE,FtiltE)  'A=fuzzy tiltE
    t1:=f.defuzzify2(Fmotor)
    f.fuzzifyA2(turn,Fturn)    'A=fuzzy turn=positive if turning right
    t2:=f.defuzzify2(Fmotor)
    motor:=-t1-t2/2'+tiltI/1500

I don't understand the code enough to know what parameters to try adjusting.

Does anyone else see what should be adjusted and where?


I've always been glad when I've taken time to try to understand Hanno's code and I'm sure I'd learn a lot from trying to understand the above method but I don't have the time at the present to do so.

I so see myself building a balancing bot in the near future (I have not previously built one) in order to understand how Hanno's code works. I think understanding Hanno's control algorithm would likely help in several of my current projects.

Mickster

Hmmm it's a bit fuzzy, isn't it :blank:

Mickster

Karpovskaia

Thats why i am asking more experienced spin programmers. I have been looking at it all day.