James Bartholomew / PidCombine

PidCombine.cpp@0:458d6472bd2d, 2019-04-11 (annotated)

Committer:
JamB
Date:
Thu Apr 11 22:17:16 2019 +0000
Revision:
0:458d6472bd2d
project;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JamB 0:458d6472bd2d 1 #include "mbed.h"
JamB 0:458d6472bd2d 2 #include "PidCombine.h"
JamB 0:458d6472bd2d 3
JamB 0:458d6472bd2d 4 void pidcombine(float rolls, float pitchs)
JamB 0:458d6472bd2d 5 {
JamB 0:458d6472bd2d 6 float roll = (rolls/2);
JamB 0:458d6472bd2d 7 float pitch = (pitchs-15);
JamB 0:458d6472bd2d 8 float top = roll+15;
JamB 0:458d6472bd2d 9 float bot = 15-roll;
JamB 0:458d6472bd2d 10 float centre = 15;
JamB 0:458d6472bd2d 11
JamB 0:458d6472bd2d 12 while (pitch != 0)
JamB 0:458d6472bd2d 13 {
JamB 0:458d6472bd2d 14 if (pitch > 0)
JamB 0:458d6472bd2d 15 {
JamB 0:458d6472bd2d 16 if (top<48)
JamB 0:458d6472bd2d 17 {
JamB 0:458d6472bd2d 18 centre = centre+1;
JamB 0:458d6472bd2d 19 pitch = pitch-1;
JamB 0:458d6472bd2d 20 top=top+1;
JamB 0:458d6472bd2d 21 }else{
JamB 0:458d6472bd2d 22 pitch=0;
JamB 0:458d6472bd2d 23 }
JamB 0:458d6472bd2d 24 }else{
JamB 0:458d6472bd2d 25 if (bot>-18)
JamB 0:458d6472bd2d 26 {
JamB 0:458d6472bd2d 27 centre=centre-1;
JamB 0:458d6472bd2d 28 pitch=pitch+1;
JamB 0:458d6472bd2d 29 bot=bot-1;
JamB 0:458d6472bd2d 30 }else{
JamB 0:458d6472bd2d 31 pitch=0;
JamB 0:458d6472bd2d 32 }
JamB 0:458d6472bd2d 33 }
JamB 0:458d6472bd2d 34 }
JamB 0:458d6472bd2d 35
JamB 0:458d6472bd2d 36 float diff=roll*2;
JamB 0:458d6472bd2d 37 centre=centre;
JamB 0:458d6472bd2d 38
JamB 0:458d6472bd2d 39 }