int main(void) {
BOARD_Init();
printf("board_inited\n");
init_MDB();
AHRS_init();
printf("ahrs inited\n");
// timer_init();
//while(1);
int i = 700;
while (i >= -700) {
while (IsReceiveEmpty());
printf("looped\n");
printf("current angle: %f\n",get_AHRS_angle());
printf("current speed: %f\n", get_AHRS_rate());
waitabit(1000);
motor_command(i);
printf("sent command %d\n\n", i);
GetChar();
i = i - 100;
}
// while (1){
// while (IsReceiveEmpty());
// printf("looped\n");
// waitabit(1000);
// printf("current speed: %d\n", motor_command(10000));
// GetChar();
// }
}
void
breakpoint (void)
{
if (initialized)
{
BREAKPOINT ();
}
waitabit ();
}
void init_MDB(void) {
TRISDbits.TRISD3 = 0;
PORTDbits.RD3 = 1;
MDBSS = 1;
waitabit(1000);
printf("done waiting\n");
printf("done waiting\n");
}
int16_t motor_command(int16_t command) {
unsigned int config = SPI_CON_MODE16 | SPI_CON_MSTEN | SPI_CON_CKE;
// the last number is the clock divider
SpiChnOpen(SPI_CHANNEL2, config, 256); //256 works // 4 doesn't //8 doesn't //16 doesn't //32 doesn't //64 doesn't //128 doesn't
MDBSS = 0;
waitabit(WAIT_TIME);
SpiChnPutC(2, command);
int16_t velocity = SpiChnGetC(2);
MDBSS = 1;
SpiChnClose(SPI_CHANNEL2);
return velocity;
}
static void
getmove( /* get view change */
XButtonPressedEvent *ebut
)
{
int movdir = MOVDIR(ebut->button);
int movorb = MOVORB(ebut->state);
int moved = 0;
Window rootw, childw;
int rootx, rooty, wx, wy;
unsigned int statemask;
copylastv( movorb ? (movdir ? "left/right" : "up/down") :
(movdir ? "fore/back" : "rotate") );
XNoOp(ourdisplay);
while (!XCheckMaskEvent(ourdisplay,
ButtonReleaseMask, levptr(XEvent))) {
/* pause so as not to move too fast */
waitabit();
if (!XQueryPointer(ourdisplay, gwind, &rootw, &childw,
&rootx, &rooty, &wx, &wy, &statemask))
break; /* on another screen */
if (!moveview(wx, vres-1-wy, movdir, movorb)) {
sleep(1);
continue;
} else
moved++;
}
if (!moved) { /* do final motion */
movdir = MOVDIR(levptr(XButtonReleasedEvent)->button);
wx = levptr(XButtonReleasedEvent)->x;
wy = levptr(XButtonReleasedEvent)->y;
moveview(wx, vres-1-wy, movdir, movorb);
}
}
void resolution(ggi_visual_t vis)
{
int xmax, ymax;
ggi_mode currmode;
ggiGetMode(vis, &currmode);
xmax = currmode.visible.x;
ymax = currmode.visible.y;
ggiSetGCForeground(vis, blue);
ggiDrawBox(vis, 0, 0, xmax, ymax);
stripevert(vis, 0, 0, xmax - 1, ymax - 1,
black, white, 1);
ggiFlush(vis);
if (waitabit(vis))
return;
stripevert(vis, 0, 0, xmax - 1, ymax - 1,
black, white, 2);
ggiFlush(vis);
if (waitabit(vis))
return;
stripevert(vis, 0, 0, xmax - 1, ymax - 1,
black, white, 3);
ggiFlush(vis);
if (waitabit(vis))
return;
stripevert(vis, 0, 0, xmax - 1, ymax - 1,
black, white, 4);
ggiFlush(vis);
if (waitabit(vis))
return;
stripehor(vis, 0, 0, xmax - 1, ymax - 1, black, white, 1);
ggiFlush(vis);
if (waitabit(vis))
return;
stripehor(vis, 0, 0, xmax - 1, ymax - 1, black, white, 2);
ggiFlush(vis);
if (waitabit(vis))
return;
starone(vis, 0, 0, xmax - 1, ymax - 1, black, white);
ggiFlush(vis);
if (waitabit(vis))
return;
starfive(vis, 0, 0, xmax - 1, ymax - 1, white, black);
ggiFlush(vis);
if (waitabit(vis))
return;
startwoten(vis, 0, 0, xmax - 1, ymax - 1, white, black);
ggiFlush(vis);
if (waitabit(vis))
return;
ggiSetGCForeground(vis, black);
ggiDrawBox(vis, 0, 0, xmax, ymax);
stripevert(vis, 1 * xmax / 16, 2 * ymax / 8, 3 * xmax / 16 - 1,
3 * ymax / 8 - 1, black, red, 4);
stripevert(vis, 3 * xmax / 16, 2 * ymax / 8, 5 * xmax / 16 - 1,
3 * ymax / 8 - 1, black, red, 3);
stripevert(vis, 5 * xmax / 16, 2 * ymax / 8, 7 * xmax / 16 - 1,
3 * ymax / 8 - 1, black, red, 2);
stripevert(vis, 7 * xmax / 16, 2 * ymax / 8, 9 * xmax / 16 - 1,
3 * ymax / 8 - 1, black, red, 1);
stripevert(vis, 9 * xmax / 16, 2 * ymax / 8, 11 * xmax / 16 - 1,
3 * ymax / 8 - 1, black, red, 2);
stripevert(vis, 11 * xmax / 16, 2 * ymax / 8, 13 * xmax / 16 - 1,
3 * ymax / 8 - 1, black, red, 3);
stripevert(vis, 13 * xmax / 16, 2 * ymax / 8, 15 * xmax / 16 - 1,
3 * ymax / 8 - 1, black, red, 4);
stripevert(vis, 1 * xmax / 16, 3 * ymax / 8, 3 * xmax / 16 - 1,
4 * ymax / 8 - 1, black, green, 4);
stripevert(vis, 3 * xmax / 16, 3 * ymax / 8, 5 * xmax / 16 - 1,
4 * ymax / 8 - 1, black, green, 3);
stripevert(vis, 5 * xmax / 16, 3 * ymax / 8, 7 * xmax / 16 - 1,
4 * ymax / 8 - 1, black, green, 2);
stripevert(vis, 7 * xmax / 16, 3 * ymax / 8, 9 * xmax / 16 - 1,
4 * ymax / 8 - 1, black, green, 1);
stripevert(vis, 9 * xmax / 16, 3 * ymax / 8, 11 * xmax / 16 - 1,
4 * ymax / 8 - 1, black, green, 2);
stripevert(vis, 11 * xmax / 16, 3 * ymax / 8, 13 * xmax / 16 - 1,
4 * ymax / 8 - 1, black, green, 3);
stripevert(vis, 13 * xmax / 16, 3 * ymax / 8, 15 * xmax / 16 - 1,
4 * ymax / 8 - 1, black, green, 4);
stripevert(vis, 1 * xmax / 16, 4 * ymax / 8, 3 * xmax / 16 - 1,
5 * ymax / 8 - 1, black, blue, 4);
stripevert(vis, 3 * xmax / 16, 4 * ymax / 8, 5 * xmax / 16 - 1,
5 * ymax / 8 - 1, black, blue, 3);
stripevert(vis, 5 * xmax / 16, 4 * ymax / 8, 7 * xmax / 16 - 1,
5 * ymax / 8 - 1, black, blue, 2);
stripevert(vis, 7 * xmax / 16, 4 * ymax / 8, 9 * xmax / 16 - 1,
5 * ymax / 8 - 1, black, blue, 1);
stripevert(vis, 9 * xmax / 16, 4 * ymax / 8, 11 * xmax / 16 - 1,
5 * ymax / 8 - 1, black, blue, 2);
stripevert(vis, 11 * xmax / 16, 4 * ymax / 8, 13 * xmax / 16 - 1,
5 * ymax / 8 - 1, black, blue, 3);
stripevert(vis, 13 * xmax / 16, 4 * ymax / 8, 15 * xmax / 16 - 1,
5 * ymax / 8 - 1, black, blue, 4);
stripevert(vis, 1 * xmax / 16, 5 * ymax / 8, 3 * xmax / 16 - 1,
6 * ymax / 8 - 1, black, white, 4);
stripevert(vis, 3 * xmax / 16, 5 * ymax / 8, 5 * xmax / 16 - 1,
6 * ymax / 8 - 1, black, white, 3);
stripevert(vis, 5 * xmax / 16, 5 * ymax / 8, 7 * xmax / 16 - 1,
6 * ymax / 8 - 1, black, white, 2);
stripevert(vis, 7 * xmax / 16, 5 * ymax / 8, 9 * xmax / 16 - 1,
6 * ymax / 8 - 1, black, white, 1);
stripevert(vis, 9 * xmax / 16, 5 * ymax / 8, 11 * xmax / 16 - 1,
6 * ymax / 8 - 1, black, white, 2);
stripevert(vis, 11 * xmax / 16, 5 * ymax / 8, 13 * xmax / 16 - 1,
6 * ymax / 8 - 1, black, white, 3);
stripevert(vis, 13 * xmax / 16, 5 * ymax / 8, 15 * xmax / 16 - 1,
6 * ymax / 8 - 1, black, white, 4);
ggiFlush(vis);
if (waitabit(vis))
return;
}
