//Main Function
int main(void)
{
double error_p = 0.0;
double error_i = 0.0;
double error_d = 0.0;
double angle;
kalman_state k = kalman_init(1, 16, 1, 137);
init_devices();
lcd_set_4bit();
lcd_init();
double preverror = 0.0;
while(1)
{
sensor2 = ADC_Conversion(2);
kalman_update(&k,(double)sensor2);
angle = k.x;
lcd_print(1,5,(int)angle,3);
error_p = (angle - 137);
error_i += error_p*dt;
error_d = (error_p - preverror)/dt;
lcd_print(1,1,abs((int)error_p),3);
Disturbance = ((error_p*kp) + (error_i*ki) + (error_d*kd));
if(angle > 137) back();
else { forward();}
if (Disturbance > MAX) Disturbance = MAX;
if (Disturbance < MIN) Disturbance = MIN;
lcd_print(2,6,abs((int)Disturbance),5);
velocity(35+abs((int)Disturbance),35+abs((int)Disturbance));
preverror = error_p;
}
}
int main (void){
float acc_x, acc_z, gyro_x;
float acc_angle,kal_angle;
/* Init Systick to 1ms */
SysTick_Config( SystemCoreClock / 1000);
/* Initialize GPIO (sets up clock) */
LPC_SYSCON->SYSAHBCLKCTRL |= (1<<6);
SERVO_init();
if(I2CInit(I2CMASTER) == FALSE){
while(1); /* fatal error */
}
if(MPU6050_init()){
return 0;
}
MPU6050_setZero();
kalman_init();
for(;;){
/* 100Hz loop */
if(gSysTick_10 >= 9){
gSysTick_10 = 0;
/* get sensor values */
gyro_x = MPU6050_getGyroRoll_degree();
acc_x = MPU6050_getAccel_x();
acc_z = -MPU6050_getAccel_z();
/* acc angle */
acc_angle = atan2(acc_x , -acc_z) * 180/3.14159 ; // calculate accel angle
kal_angle = kalman_update(acc_angle,gyro_x, 0.01);
SERVO_set_slew((-kal_angle) - MECH_OFFSET);
}
}
}
int main(int argc, char **argv)
{
int i;
float gyro_input;
float accel_input;
float kalman_output;
for (i = 0; i < SAMPLE_COUNT; ++i)
{
// Get the gyro and accelerometer input.
gyro_input = sample_data[i][0];
accel_input = sample_data[i][1];
// Update the Kalman filter and get the output.
kalman_output = kalman_update(gyro_input, accel_input);
// Print out input data and the kalman output.
printf("%d gyro=%7.3f deg/sec accel=%7.3f deg kalman_output=%5.1f deg\n",
i, gyro_input, accel_input, kalman_output);
}
return 0;
}
/*****************************************************************************
* Balance -
*****************************************************************************/
void balance(void)
{
unsigned long TimerMsWork;
long int g_bias = 0;
double x_offset = 532; //offset value 2.56V * 1024 / 4.93V = 4254
double q_m = 0.0;
double int_angle = 0.0;
double x = 0.0;
double tilt = 0.0;
int pwm;
InitADC();
init_pwm();
// initialize the UART (serial port)
uartInit();
// set the baud rate of the UART for our debug/reporting output
uartSetBaudRate(115200);
// initialize rprintf system
rprintfInit(uartSendByte);
// initialize vt100 library
vt100Init();
// clear the terminal screen
vt100ClearScreen();
TimerMsWork = TimerMsCur();
DDRB |= (1 << PB0); // Make B0 an output for LED
/* as a 1st step, a reference measurement of the angular rate sensor is
* done. This value is used as offset compensation */
for (int i=1 ; i<=200; i++) // determine initial value for bias of gyro
{
g_bias = g_bias + GetADC(gyro_sensor);
}
g_bias = g_bias / 200;
while (!(getkey() == 1))
{
/* insure loop runs at specified Hz */
while (!TimerCheck(TimerMsWork, (dt_PARAM * 1000) -1))
;
TimerMsWork = TimerMsCur();
// toggle pin B0 for oscilloscope timings.
PORTB = PINB ^ (1 << PB0);
// get rate gyro reading and convert to deg/sec
// q_m = (GetADC(gyro_sensor) - g_bias) / -3.072; // -3.07bits/deg/sec (neg. because forward is CCW)
q_m = (GetADC(gyro_sensor) - g_bias) * -0.3255; // each bit = 0.3255 /deg/sec (neg. because forward is CCW)
state_update(q_m);
// get Accelerometer reading and convert to units of gravity.
// x = (GetADC(accel_sensor) - x_offset) / 204.9; // (205 bits/G)
x = (GetADC(accel_sensor) - x_offset) * 0.00488; // each bit = 0.00488/G
// x is measured in multiples of earth gravitation g
// therefore x = sin (tilt) or tilt = arcsin(x)
// for small angles in rad (not deg): arcsin(x)=x
// Calculation of deg from rad: 1 deg = 180/pi = 57.29577951
tilt = 57.29577951 * (x);
kalman_update(tilt);
int_angle += angle * dt_PARAM;
rprintf(" x:");
rprintfFloat(8, x);
rprintf(" angle:");
rprintfFloat(8, angle);
rprintf(" rate:");
rprintfFloat(8, rate);
// Balance. The most important line in the entire program.
// balance_torque = Kp * (current_angle - neutral) + Kd * current_rate;
// rprintf("bal_torq: ");
// rprintfFloat(8, balance_torque);
// rprintfCRLF();
//steer_knob = 0;
// change from current angle to something proportional to speed
// should this be the abs val of the cur speed or just curr speed?
//double steer_cmd = (1.0 / (1.0 + Ksteer2 * fabs(current_angle))) * (Ksteer * steer_knob);
//double steer_cmd = 0.0;
// Get current rate of turn
//double current_turn = left_speed - right_speed; //<-- is this correct
//double turn_accel = current_turn - prev_turn;
//prev_turn = current_turn;
// Closed-loop turn rate PID
//double steer_cmd = KpTurn * (current_turn - steer_desired)
// + KdTurn * turn_accel;
// //+ KiTurn * turn_integrated;
// Possibly optional
//turn_integrated += current_turn - steer_cmd;
// Differential steering
//left_motor_torque = balance_torque + steer_cmd; //+ cur_speed + steer_cmd;
//right_motor_torque = balance_torque - steer_cmd; //+ cur_speed - steer_cmd;
// Limit extents of torque demand
//left_motor_torque = flim(left_motor_torque, -MAX_TORQUE, MAX_TORQUE);
// if (left_motor_torque < -MAX_TORQUE) left_motor_torque = -MAX_TORQUE;
// if (left_motor_torque > MAX_TORQUE) left_motor_torque = MAX_TORQUE;
//right_motor_torque = flim(right_motor_torque, -MAX_TORQUE, MAX_TORQUE);
// if (right_motor_torque < -MAX_TORQUE) right_motor_torque = -MAX_TORQUE;
// if (right_motor_torque > MAX_TORQUE) right_motor_torque = MAX_TORQUE;
pwm = (int) ((angle -3.5) * Kp) + (rate * Kd); // + (int_angle * Ki);
rprintf(" pwm:%d\r\n", pwm);
// Set PWM values for both motors
SetLeftMotorPWM(pwm);
SetRightMotorPWM(pwm);
}
SetLeftMotorPWM(0);
SetRightMotorPWM(0);
}
/**
Time domain physical simulation.
noisy:
- 0: no noise at all;
- 1: poisson and read out noise.
- 2: only poisson noise.
*/
dmat *skysim_sim(dmat **mresout, const dmat *mideal, const dmat *mideal_oa, double ngsol,
ASTER_S *aster, const POWFS_S *powfs,
const PARMS_S *parms, int idtratc, int noisy, int phystart){
int dtratc=0;
if(!parms->skyc.multirate){
dtratc=parms->skyc.dtrats->p[idtratc];
}
int hasphy;
if(phystart>-1 && phystart<aster->nstep){
hasphy=1;
}else{
hasphy=0;
}
const int nmod=mideal->nx;
dmat *res=dnew(6,1);/*Results. 1-2: NGS and TT modes.,
3-4:On axis NGS and TT modes,
4-6: On axis NGS and TT wihtout considering un-orthogonality.*/
dmat *mreal=NULL;/*modal correction at this step. */
dmat *merr=dnew(nmod,1);/*modal error */
dcell *merrm=dcellnew(1,1);dcell *pmerrm=NULL;
const int nstep=aster->nstep?aster->nstep:parms->maos.nstep;
dmat *mres=dnew(nmod,nstep);
dmat* rnefs=parms->skyc.rnefs;
dcell *zgradc=dcellnew3(aster->nwfs, 1, aster->ngs, 0);
dcell *gradout=dcellnew3(aster->nwfs, 1, aster->ngs, 0);
dmat *gradsave=0;
if(parms->skyc.dbg){
gradsave=dnew(aster->tsa*2,nstep);
}
SERVO_T *st2t=0;
kalman_t *kalman=0;
dcell *mpsol=0;
dmat *pgm=0;
dmat *dtrats=0;
int multirate=parms->skyc.multirate;
if(multirate){
kalman=aster->kalman[0];
dtrats=aster->dtrats;
}else{
if(parms->skyc.servo>0){
const double dtngs=parms->maos.dt*dtratc;
st2t=servo_new(merrm, NULL, 0, dtngs, aster->gain->p[idtratc]);
pgm=aster->pgm->p[idtratc];
}else{
kalman=aster->kalman[idtratc];
}
}
if(kalman){
kalman_init(kalman);
mpsol=dcellnew(aster->nwfs, 1); //for psol grad.
}
const long nwvl=parms->maos.nwvl;
dcell **psf=0, **mtche=0, **ints=0;
ccell *wvf=0,*wvfc=0, *otf=0;
if(hasphy){
psf=mycalloc(aster->nwfs,dcell*);
wvf=ccellnew(aster->nwfs,1);
wvfc=ccellnew(aster->nwfs,1);
mtche=mycalloc(aster->nwfs,dcell*);
ints=mycalloc(aster->nwfs,dcell*);
otf=ccellnew(aster->nwfs,1);
for(long iwfs=0; iwfs<aster->nwfs; iwfs++){
const int ipowfs=aster->wfs[iwfs].ipowfs;
const long ncomp=parms->maos.ncomp[ipowfs];
const long nsa=parms->maos.nsa[ipowfs];
wvf->p[iwfs]=cnew(ncomp,ncomp);
wvfc->p[iwfs]=NULL;
psf[iwfs]=dcellnew(nsa,nwvl);
//cfft2plan(wvf->p[iwfs], -1);
if(parms->skyc.multirate){
mtche[iwfs]=aster->wfs[iwfs].pistat->mtche[(int)aster->idtrats->p[iwfs]];
}else{
mtche[iwfs]=aster->wfs[iwfs].pistat->mtche[idtratc];
}
otf->p[iwfs]=cnew(ncomp,ncomp);
//cfft2plan(otf->p[iwfs],-1);
//cfft2plan(otf->p[iwfs],1);
ints[iwfs]=dcellnew(nsa,1);
int pixpsa=parms->skyc.pixpsa[ipowfs];
for(long isa=0; isa<nsa; isa++){
ints[iwfs]->p[isa]=dnew(pixpsa,pixpsa);
}
}
}
for(int irep=0; irep<parms->skyc.navg; irep++){
if(kalman){
kalman_init(kalman);
}else{
servo_reset(st2t);
}
dcellzero(zgradc);
dcellzero(gradout);
if(ints){
for(int iwfs=0; iwfs<aster->nwfs; iwfs++){
dcellzero(ints[iwfs]);
}
}
for(int istep=0; istep<nstep; istep++){
memcpy(merr->p, PCOL(mideal,istep), nmod*sizeof(double));
dadd(&merr, 1, mreal, -1);/*form NGS mode error; */
memcpy(PCOL(mres,istep),merr->p,sizeof(double)*nmod);
if(mpsol){//collect averaged modes for PSOL.
for(long iwfs=0; iwfs<aster->nwfs; iwfs++){
dadd(&mpsol->p[iwfs], 1, mreal, 1);
}
}
pmerrm=0;
if(istep>=parms->skyc.evlstart){/*performance evaluation*/
double res_ngs=dwdot(merr->p,parms->maos.mcc,merr->p);
if(res_ngs>ngsol*100){
dfree(res); res=NULL;
break;
}
{
res->p[0]+=res_ngs;
res->p[1]+=dwdot2(merr->p,parms->maos.mcc_tt,merr->p);
double dot_oa=dwdot(merr->p, parms->maos.mcc_oa, merr->p);
double dot_res_ideal=dwdot(merr->p, parms->maos.mcc_oa, PCOL(mideal,istep));
double dot_res_oa=0;
for(int imod=0; imod<nmod; imod++){
dot_res_oa+=merr->p[imod]*IND(mideal_oa,imod,istep);
}
res->p[2]+=dot_oa-2*dot_res_ideal+2*dot_res_oa;
res->p[4]+=dot_oa;
}
{
double dot_oa_tt=dwdot2(merr->p, parms->maos.mcc_oa_tt, merr->p);
/*Notice that mcc_oa_tt2 is 2x5 marix. */
double dot_res_ideal_tt=dwdot(merr->p, parms->maos.mcc_oa_tt2, PCOL(mideal,istep));
double dot_res_oa_tt=0;
for(int imod=0; imod<2; imod++){
dot_res_oa_tt+=merr->p[imod]*IND(mideal_oa,imod,istep);
}
res->p[3]+=dot_oa_tt-2*dot_res_ideal_tt+2*dot_res_oa_tt;
res->p[5]+=dot_oa_tt;
}
}//if evl
if(istep<phystart || phystart<0){
/*Ztilt, noise free simulation for acquisition. */
dmm(&zgradc->m, 1, aster->gm, merr, "nn", 1);/*grad due to residual NGS mode. */
for(int iwfs=0; iwfs<aster->nwfs; iwfs++){
const int ipowfs=aster->wfs[iwfs].ipowfs;
const long ng=parms->maos.nsa[ipowfs]*2;
for(long ig=0; ig<ng; ig++){
zgradc->p[iwfs]->p[ig]+=aster->wfs[iwfs].ztiltout->p[istep*ng+ig];
}
}
for(int iwfs=0; iwfs<aster->nwfs; iwfs++){
int dtrati=(multirate?(int)dtrats->p[iwfs]:dtratc);
if((istep+1) % dtrati==0){
dadd(&gradout->p[iwfs], 0, zgradc->p[iwfs], 1./dtrati);
dzero(zgradc->p[iwfs]);
if(noisy){
int idtrati=(multirate?(int)aster->idtrats->p[iwfs]:idtratc);
dmat *nea=aster->wfs[iwfs].pistat->sanea->p[idtrati];
for(int i=0; i<nea->nx; i++){
gradout->p[iwfs]->p[i]+=nea->p[i]*randn(&aster->rand);
}
}
pmerrm=merrm;//record output.
}
}
}else{
/*Accumulate PSF intensities*/
for(long iwfs=0; iwfs<aster->nwfs; iwfs++){
const double thetax=aster->wfs[iwfs].thetax;
const double thetay=aster->wfs[iwfs].thetay;
const int ipowfs=aster->wfs[iwfs].ipowfs;
const long nsa=parms->maos.nsa[ipowfs];
ccell* wvfout=aster->wfs[iwfs].wvfout[istep];
for(long iwvl=0; iwvl<nwvl; iwvl++){
double wvl=parms->maos.wvl[iwvl];
for(long isa=0; isa<nsa; isa++){
ccp(&wvfc->p[iwfs], IND(wvfout,isa,iwvl));
/*Apply NGS mode error to PSF. */
ngsmod2wvf(wvfc->p[iwfs], wvl, merr, powfs+ipowfs, isa,
thetax, thetay, parms);
cembedc(wvf->p[iwfs],wvfc->p[iwfs],0,C_FULL);
cfft2(wvf->p[iwfs],-1);
/*peak in corner. */
cabs22d(&psf[iwfs]->p[isa+nsa*iwvl], 1., wvf->p[iwfs], 1.);
}/*isa */
}/*iwvl */
}/*iwfs */
/*Form detector image from accumulated PSFs*/
double igrad[2];
for(long iwfs=0; iwfs<aster->nwfs; iwfs++){
int dtrati=dtratc, idtrat=idtratc;
if(multirate){//multirate
idtrat=aster->idtrats->p[iwfs];
dtrati=dtrats->p[iwfs];
}
if((istep+1) % dtrati == 0){/*has output */
dcellzero(ints[iwfs]);
const int ipowfs=aster->wfs[iwfs].ipowfs;
const long nsa=parms->maos.nsa[ipowfs];
for(long isa=0; isa<nsa; isa++){
for(long iwvl=0; iwvl<nwvl; iwvl++){
double siglev=aster->wfs[iwfs].siglev->p[iwvl];
ccpd(&otf->p[iwfs],psf[iwfs]->p[isa+nsa*iwvl]);
cfft2i(otf->p[iwfs], 1); /*turn to OTF, peak in corner */
ccwm(otf->p[iwfs], powfs[ipowfs].dtf[iwvl].nominal);
cfft2(otf->p[iwfs], -1);
dspmulcreal(ints[iwfs]->p[isa]->p, powfs[ipowfs].dtf[iwvl].si,
otf->p[iwfs]->p, siglev);
}
/*Add noise and apply matched filter. */
#if _OPENMP >= 200805
#pragma omp critical
#endif
switch(noisy){
case 0:/*no noise at all. */
break;
case 1:/*both poisson and read out noise. */
{
double bkgrnd=aster->wfs[iwfs].bkgrnd*dtrati;
addnoise(ints[iwfs]->p[isa], &aster->rand, bkgrnd, bkgrnd, 0,0,IND(rnefs,idtrat,ipowfs));
}
break;
case 2:/*there is still poisson noise. */
addnoise(ints[iwfs]->p[isa], &aster->rand, 0, 0, 0,0,0);
break;
default:
error("Invalid noisy\n");
}
igrad[0]=0;
igrad[1]=0;
double pixtheta=parms->skyc.pixtheta[ipowfs];
if(parms->skyc.mtch){
dmulvec(igrad, mtche[iwfs]->p[isa], ints[iwfs]->p[isa]->p, 1);
}
if(!parms->skyc.mtch || fabs(igrad[0])>pixtheta || fabs(igrad[1])>pixtheta){
if(!parms->skyc.mtch){
warning2("fall back to cog\n");
}else{
warning_once("mtch is out of range\n");
}
dcog(igrad, ints[iwfs]->p[isa], 0, 0, 0, 3*IND(rnefs,idtrat,ipowfs), 0);
igrad[0]*=pixtheta;
igrad[1]*=pixtheta;
}
gradout->p[iwfs]->p[isa]=igrad[0];
gradout->p[iwfs]->p[isa+nsa]=igrad[1];
}/*isa */
pmerrm=merrm;
dcellzero(psf[iwfs]);/*reset accumulation.*/
}/*if iwfs has output*/
}/*for wfs*/
}/*if phystart */
//output to mreal after using it to ensure two cycle delay.
if(st2t){//Type I or II control.
if(st2t->mint->p[0]){//has output.
dcp(&mreal, st2t->mint->p[0]->p[0]);
}
}else{//LQG control
kalman_output(kalman, &mreal, 0, 1);
}
if(kalman){//LQG control
int indk=0;
//Form PSOL grads and obtain index to LQG M
for(int iwfs=0; iwfs<aster->nwfs; iwfs++){
int dtrati=(multirate?(int)dtrats->p[iwfs]:dtratc);
if((istep+1) % dtrati==0){
indk|=1<<iwfs;
dmm(&gradout->p[iwfs], 1, aster->g->p[iwfs], mpsol->p[iwfs], "nn", 1./dtrati);
dzero(mpsol->p[iwfs]);
}
}
if(indk){
kalman_update(kalman, gradout->m, indk-1);
}
}else if(st2t){
if(pmerrm){
dmm(&merrm->p[0], 0, pgm, gradout->m, "nn", 1);
}
servo_filter(st2t, pmerrm);//do even if merrm is zero. to simulate additional latency
}
if(parms->skyc.dbg){
memcpy(PCOL(gradsave, istep), gradout->m->p, sizeof(double)*gradsave->nx);
}
}/*istep; */
}
if(parms->skyc.dbg){
int dtrati=(multirate?(int)dtrats->p[0]:dtratc);
writebin(gradsave,"%s/skysim_grads_aster%d_dtrat%d",dirsetup, aster->iaster,dtrati);
writebin(mres,"%s/skysim_sim_mres_aster%d_dtrat%d",dirsetup,aster->iaster,dtrati);
}
dfree(mreal);
dcellfree(mpsol);
dfree(merr);
dcellfree(merrm);
dcellfree(zgradc);
dcellfree(gradout);
dfree(gradsave);
if(hasphy){
dcellfreearr(psf, aster->nwfs);
dcellfreearr(ints, aster->nwfs);
ccellfree(wvf);
ccellfree(wvfc);
ccellfree(otf);
free(mtche);
}
servo_free(st2t);
/*dfree(mres); */
if(mresout) {
*mresout=mres;
}else{
dfree(mres);
}
dscale(res, 1./((nstep-parms->skyc.evlstart)*parms->skyc.navg));
return res;
}
/**
* ahrs_step() takes the values from the IMU and produces the
* new estimated attitude.
*/
void
ahrs_step(
v_t angles_out,
f_t dt,
f_t p,
f_t q,
f_t r,
f_t ax,
f_t ay,
f_t az,
f_t heading
)
{
m_t C;
m_t A;
m_t AT;
v_t X_dot;
m_t temp;
v_t Xm;
v_t Xe;
/* Construct the quaternion omega matrix */
rotate2omega( A, p, q, r );
/* X_dot = AX */
mv_mult( X_dot, A, X, 4, 4 );
/* X += X_dot dt */
v_scale( X_dot, X_dot, dt, 4 );
v_add( X, X_dot, X, 4 );
v_normq( X, X );
/*
printf( "X =" );
Vprint( X, 4 );
printf( "\n" );
*/
/* AT = transpose(A) */
m_transpose( AT, A, 4, 4 );
/* P = A * P * AT + Q */
m_mult( temp, A, P, 4, 4, 4 );
m_mult( P, temp, AT, 4, 4, 4 );
m_add( P, Q, P, 4, 4 );
compute_c( C, X );
/* Compute the euler angles of the measured attitude */
accel2euler(
Xm,
ax,
ay,
az,
heading
);
/*
* Compute the euler angles of the estimated attitude
* Xe = quat2euler( X )
*/
quat2euler( Xe, X );
/* Kalman filter update */
kalman_update(
P,
R,
C,
X,
Xe,
Xm
);
/* Estimated attitude extraction */
quat2euler( angles_out, X );
}
