hschumann2/TempleOS-Source-Code

1 2/*If you find this interesting, you might want to check-out3SimStructure, a simulation program I wrote you can downlload.4 5/files/SimStrSetUp.zip6 7I did study ctrl systems for 2 years, but lost enthusiasm8when I learned everything in real-life is nonlinear.9You're supposed to linearize around points and apply10control theory.  I just like to appy a PID ctrller11and fiddle intelligently with the feedback coefficients.12 13Real rockets have dynamics like fuel slosh to consider!14*/15 16#define THRUST  100017 18Bool    blast_off,plane_hit;19 20CMass   m1, //Bottom of rocket21        m2, //Top of rocket22        m3; //Plane23CSpring s;24 25#define ROCKET_HEIGHT   4026#define GROUND_Y        (GR_HEIGHT-3*FONT_HEIGHT)27 28 29   <1>/* Graphics Not Rendered in HTML */30 31 32 33   <2>/* Graphics Not Rendered in HTML */34 35 36 37  <3>/* Graphics Not Rendered in HTML */38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54/* Graphics Not Rendered in HTML */55 56CDC     *dc2;57CMathODE *ode;58 59#define STATE_NOZZLE_ANGLE              060#define STATE_NOZZLE_ANGLE_VELOCITY     161#define STATE_NUM                       262 63CD3     target;64F64     my_dbg,antispin_coefficient;65 66U0 DrawIt(CTask *task,CDC *dc)67{68  I64 i,x,y,cx=GR_WIDTH/2,cy=GROUND_Y;69  F64 theta=Arg(m2.x-m1.x,m2.y-m1.y),70        nozzle_angle=ode->state[STATE_NOZZLE_ANGLE];71 72  if (blast_off) {73    x=m1.x-10*Cos(theta+nozzle_angle);74    y=m1.y-10*Sin(theta+nozzle_angle);75    for (i=0;i<6;i++) {76      if ((i^winmgr.updates)&1)77        dc->color=YELLOW;78      else79        dc->color=RED;80      GrLine(dc,cx+(m1.x+i*Cos(theta-pi/2)),cy-(m1.y+i*Sin(theta-pi/2)),cx+x,cy-y);81      GrLine(dc,cx+(m1.x+i*Cos(theta+pi/2)),cy-(m1.y+i*Sin(theta+pi/2)),cx+x,cy-y);82    }83 84    for (i=0;i<10;i++) {85      switch (RandU16&3) {86        case 0: dc2->color=WHITE;       break;87        case 1: dc2->color=LTGRAY;      break;88        case 2: dc2->color=DKGRAY;      break;89        case 3: dc2->color=BLACK;       break;90      }91      GrPlot(dc2,cx+(x+RandU16%12-6),cy-(y+RandU16%12-6));92    }93  }94 95  if (plane_hit)96    Sprite3(dc,cx+m3.x,cy-m3.y,0,<2>);97  else98    Sprite3(dc,cx+m3.x,cy-m3.y,0,<1>);99 100  if (blast_off && !plane_hit) {101    dc->color=ROP_COLLISION;102    dc->bkcolor=LTCYAN;103    dc->collision_cnt=0;104    Sprite3ZB(dc,cx+(m1.x+m2.x)/2,cy-(m1.y+m2.y)/2,0,<3>,-theta);105    if (dc->collision_cnt>100) {106      Noise(1000,62,81);107      plane_hit=TRUE;108    } else109      Snd(22);110  } else if (!plane_hit)111    Snd;112 113  dc->color=ROP_EQU;114  Sprite3(dc,0,GROUND_Y,0,<4>);115  Sprite3ZB(dc,cx+(m1.x+m2.x)/2,cy-(m1.y+m2.y)/2,0,<3>,-theta);116 117  dc->color=RED;118  GrCircle(dc,cx+target.x,cy-target.y,5);119 120  dc->color=BLUE;121  GrCircle(dc,cx+m3.x,cy-m3.y,5);122 123  dc->color=BLACK;124  GrPrint(dc,0,FONT_HEIGHT,"%12.6f",my_dbg);125}126 127U0 MyDerivative(CMathODE *,F64,F64 *state,F64 *DstateDt)128{129  F64 d,discriminant,v,a,theta=Arg(m2.state->x-m1.state->x,m2.state->y-m1.state->y),130        DthetaDt,collision_estimate_t,target_heading,target_angle_error,131        desired_nozzle_angle;132  CD3 p,p_target,p_body;133 134  //Unit vect pointing to top of rocket from bottom.135  D3Sub(&p_body,&m2.state->x,&m1.state->x);136  D3Unit(&p_body);137 138  //DthetaDt lets us prevent too much spin.139  DthetaDt=antispin_coefficient*140        (m2.state->DyDt*p_body.x-m2.state->DxDt*p_body.y-141        m1.state->DyDt*p_body.x+m1.state->DxDt*p_body.y)/ROCKET_HEIGHT;142 143  //p_target is vect from top of rocket to plane.144  D3Sub(&p_target,&m3.state->x,&m2.state->x);145 146  //d=0.5at^2+vt147  d=D3Norm(&p_target);148 149  D3Copy(&p,&p_target);150  D3Unit(&p);151  v=(m2.state->DxDt*p.x+m2.state->DyDt*p.y)-152        (m3.state->DxDt*p.x+m3.state->DyDt*p.y);153 154  a=THRUST/(m1.mass+m2.mass);155 156  discriminant=v*v+4*0.5*a*d;157  if (discriminant>0)158    collision_estimate_t=(-v+Sqrt(discriminant))/a;159  else160    collision_estimate_t=0;161  my_dbg=collision_estimate_t;162 163  //Aim for projected pos of plane at time of impact.164  D3Copy(&p,&m3.state->DxDt);165  D3MulEqu(&p,collision_estimate_t);166  D3AddEqu(&p_target,&p);167 168  D3Copy(&target,&p_target);169  D3AddEqu(&target,&m2.state->x);170 171  target_heading=Arg(p_target.x,p_target.y);172  target_angle_error=Wrap(theta-target_heading); //Force to range [-pi,pi)173  desired_nozzle_angle=Clamp(50.0*DthetaDt+750*target_angle_error,-pi/8,pi/8);174 175  //For realism we limit the speed the nozzle angle can change.176  DstateDt[STATE_NOZZLE_ANGLE]=state[STATE_NOZZLE_ANGLE_VELOCITY];177  DstateDt[STATE_NOZZLE_ANGLE_VELOCITY]=178        Clamp(10000*(desired_nozzle_angle-state[STATE_NOZZLE_ANGLE]),179        -1000,1000)-10.0*state[STATE_NOZZLE_ANGLE_VELOCITY]; //Damping180 181  if (blast_off) {182    m1.DstateDt->DxDt+=THRUST*Cos(theta+state[STATE_NOZZLE_ANGLE]);183    m1.DstateDt->DyDt+=THRUST*Sin(theta+state[STATE_NOZZLE_ANGLE]);184 185    m1.DstateDt->DyDt-=25; //Gravity186    m2.DstateDt->DyDt-=25;187  }188 189  //For more realism reduce the mass of the rocket because of fuel.190  //You might also factor-in fuel slosh in the tank.191 192  //To do this, you would have to set-up state vars for mass and193  //do A=F/m manually instead of relyin on ODECallDerivative() to divide194  //by mass.195}196 197U0 Init()198{199  DocClear;200  "$BG,LTCYAN$%h*c",ToI64(GROUND_Y/FONT_HEIGHT),'\n';201 202  blast_off=FALSE;203  plane_hit=FALSE;204 205  do antispin_coefficient=PopUpRangeF64Exp(0.1,10.001,Sqrt(10),206          "%9.4f","Anti-spin Coefficient\n\n");207  while (!(0.1<=antispin_coefficient<10.001));208 209  //We don't clear que links.210  MemSet(&m1.start,0,offset(CMass.end)-offset(CMass.start));211  m1.y=0;212  m1.mass=1.0;213 214  MemSet(&m2.start,0,offset(CMass.end)-offset(CMass.start));215  m2.y=ROCKET_HEIGHT;216  m2.mass=1.0;217 218  MemSet(&m3.start,0,offset(CMass.end)-offset(CMass.start));219  m3.y=400;220  m3.x=-300;221  m3.DxDt=50;222  m3.mass=1.0;223 224  MemSet(&s.start,0,offset(CSpring.end)-offset(CSpring.start));225  s.end1=&m1;226  s.end2=&m2;227  s.rest_len=ROCKET_HEIGHT;228  s.const=10000;229 230  ode->state[STATE_NOZZLE_ANGLE]=0;231  ode->state[STATE_NOZZLE_ANGLE_VELOCITY]=0;232 233  DCFill;234}235 236U0 TaskEndCB()237{238  DCFill;239  SndTaskEndCB;240}241 242U0 RocketScience()243{244  SettingsPush; //See SettingsPush245  Fs->text_attr=YELLOW<<4+BLUE;246  MenuPush(247        "File {"248        "  Abort(,CH_SHIFT_ESC);"249        "  Exit(,CH_ESC);"250        "}"251        "Play {"252        "  Restart(,'\n');"253        "  Launch(,CH_SPACE);"254        "}"255        );256 257  AutoComplete;258  WinBorder;259  WinMax;260  DocCursor;261  DocClear;262  dc2=DCAlias;263  Fs->task_end_cb=&TaskEndCB;264 265  ode=ODENew(STATE_NUM,1e-6,ODEF_HAS_MASSES);266  ode->derive=&MyDerivative;267  ode->drag_v2=0.002;268  ode->drag_v3=0.00001;269  ode->acceleration_limit=5e3;270 271  //  ode->t_scale=0.1; //Uncomment this to go in slow motion.272 273  Init;274  QueIns(&m1,ode->last_mass);275  QueIns(&m2,ode->last_mass);276  QueIns(&m3,ode->last_mass);277  QueIns(&s,ode->last_spring);278 279  QueIns(ode,Fs->last_ode);280 281  Fs->draw_it=&DrawIt;282 283  try {284    GetKey;285    blast_off=TRUE;286    while (TRUE) {287      switch (GetChar(,FALSE)) {288        case '\n':289          Init;290          GetKey;291          blast_off=TRUE;292          break;293        case CH_SHIFT_ESC:294        case CH_ESC:295          goto rs_done;296      }297    }298rs_done:299  } catch300    PutExcept;301  QueRem(ode);302  ODEDel(ode);303  DocClear;304  SettingsPop;305  DCFill;306  DCDel(dc2);307  MenuPop;308}309 310RocketScience;311