void serverms(int mode, int numplayers, int minremain, char *smapname, int seconds, bool isfull) {
checkmasterreply();
updatemasterserver(seconds);
// reply all server info requests
ENetBuffer buf;
ENetAddress addr;
u8 pong[MAXTRANS], *p;
int len;
enet_uint32 events = ENET_SOCKET_WAIT_RECEIVE;
buf.data = pong;
while (enet_socket_wait(pongsock, &events, 0) >= 0 && events) {
buf.dataLength = sizeof(pong);
len = enet_socket_receive(pongsock, &addr, &buf, 1);
if (len < 0) return;
p = &pong[len];
putint(p, PROTOCOL_VERSION);
putint(p, mode);
putint(p, numplayers);
putint(p, minremain);
string mname;
strcpy_s(mname, isfull ? "[FULL] " : "");
strcat_s(mname, smapname);
sendstring(mname, p);
sendstring(serverdesc, p);
buf.dataLength = p - pong;
enet_socket_send(pongsock, &addr, &buf, 1);
}
}
void h_vdi_call(VDIPB *vdipb,short mode) {
if((vdipb->contrl[0] == 1) || (vdipb->contrl[0] == 101)) {
int i;
for(i = 0; i < 11; i++) {
sprintf(data,"work_in[%d]=%d\r\n",i,vdipb->intin[i]);
sendstring(data);
};
sprintf(data,"handle=%d\r\n",vdipb->contrl[6]);
sendstring(data);
};
}
PPTE_PAE mapAddressAtPML4(QWORD address)
{
static criticalSection CS;
int index;
csEnter(&CS);
sendstringf("mapAddressAtPML4(%6)\n", address);
index=getFreePML4Index();
if (index==-1)
{
sendstring("No PML4 entries free\n");
csLeave(&CS);
return NULL;
}
*(QWORD*)(&pml4table[index])=address;
pml4table[index].P=1;
pml4table[index].RW=1;
asm volatile ("": : :"memory");
csLeave(&CS);
QWORD result=((QWORD)index << 39);
if (result>=0x800000000000ULL) //sign extend
result|=0xffff000000000000ULL;
_invlpg(result);
_wbinvd();
return (PPTE_PAE)result;
}
void sendmap(char *mapname)
{
if(*mapname) save_world(mapname);
changemap(mapname);
mapname = getclientmap();
int mapsize;
uchar *mapdata = readmap(mapname, &mapsize);
if(!mapdata) return;
ENetPacket *packet = enet_packet_create(NULL, MAXTRANS + mapsize, ENET_PACKET_FLAG_RELIABLE);
uchar *start = packet->data;
uchar *p = start+2;
putint(p, SV_SENDMAP);
sendstring(mapname, p);
putint(p, mapsize);
if(65535 - (p - start) < mapsize)
{
conoutf("map %s is too large to send", mapname);
free(mapdata);
enet_packet_destroy(packet);
return;
};
memcpy(p, mapdata, mapsize);
p += mapsize;
free(mapdata);
*(ushort *)start = ENET_HOST_TO_NET_16(p-start);
enet_packet_resize(packet, p-start);
sendpackettoserv(packet);
conoutf("sending map %s to server...", mapname);
sprintf_sd(msg)("[map %s uploaded to server, \"getmap\" to receive it]", mapname);
toserver(msg);
}
void log_printf(char* format,...)
{
va_list args;
if(log_start == LOG_ENABLE) {
va_start(args,format);
vsprintf(buffer,format,args);
sendstring(buffer);
va_end(args);
}
}
void main(void) {
Supexec(link_in);
printf("TrackVDI installed.\n");
sprintf(data,"\r\n\r\nTrackVDI started.\r\n\r\n");
sendstring(data);
Ptermres((long)_base->p_dbase + _base->p_dlen - (long)_base->p_lowtpa,0);
}
void analise_command()
{
if (toupper(recvbuff[0]) == 'S' && toupper(recvbuff[1]) == 'T') {stop_pwm(); return;}
else if (toupper(recvbuff[0]) == 'T' && toupper(recvbuff[1]) == '+') {throttle_up(); return;}
else if (toupper(recvbuff[0]) == 'T' && toupper(recvbuff[1]) == '-') {throttle_down(); return;}
else if (toupper(recvbuff[0]) == 'C' && toupper(recvbuff[1]) == 'L') {sensor_calib(); return;}
else if (toupper(recvbuff[0]) == 'C' && toupper(recvbuff[1]) == 'N') {init(); sendstring("CONNECT\0"); return;}
else if (toupper(recvbuff[0]) == 'T' && toupper(recvbuff[1]) == 'M') {sendtemp(); return;}
else if (toupper(recvbuff[0]) == 'P' && toupper(recvbuff[1]) == 'R') {sendpress(); return;}
}
void sendservmsg(const char *msg) {
ENetPacket *packet = enet_packet_create(NULL, MAXDEFSTR+10, ENET_PACKET_FLAG_RELIABLE);
u8 *start = packet->data;
u8 *p = start+2;
putint(p, SV_SERVMSG);
sendstring(msg, p);
*(u16 *)start = ENET_HOST_TO_NET_16(int(p-start));
enet_packet_resize(packet, p-start);
multicast(packet, -1);
if (packet->referenceCount==0) enet_packet_destroy(packet);
}
void main(void)
{
char b[7];
b[0]='0'+rand()%10;
b[1]=' ';
b[2]='0'+rand()%10;
b[3]=' ';
b[4]='0';
b[5]='\n';
b[6]='\0';
int i=0;
int adc_val2;
WDTCTL = WDTPW + WDTHOLD; // Stop WDT
P1DIR |= (BIT6); // Set the LEDs on P1.0, P1.1, P1.2 and P1.6 as outputs
P1OUT = 0x10;
BCSCTL1 = CALBC1_1MHZ; // Set DCO
DCOCTL = CALDCO_1MHZ;
P1SEL = BIT1 + BIT2 ; // P1.1 = RXD, P1.2=TXD
P1SEL2 = BIT1 + BIT2 ; // P1.1 = RXD, P1.2=TXD
UCA0CTL1 |= UCSSEL_2; // SMCLK
UCA0BR0 = 104; // 1MHz 9600
UCA0BR1 = 0; // 1MHz 9600
UCA0MCTL = UCBRS0; // Modulation UCBRSx = 1
UCA0CTL1 &= ~UCSWRST; // **Initialize USCI state machine**
IE2 |= UCA0RXIE; // Enable USCI_A0 RX interrupt
while(1)
{
ADC10CTL0 = SREF_0+ADC10SHT_2 + ADC10ON; // Use Vcc/Vss for Up/Low Refs, 16 x ADC10CLKs, turn on ADC
ADC10CTL1 = INCH_0+SHS_0 + ADC10SSEL_0 + ADC10DIV_0 + CONSEQ_0;
ADC10AE0 |= 0x10 ; // PA.0 ADC option select
ADC10CTL0 |= ENC + ADC10SC; // Sampling and conversion start
adc_val2=ADC10MEM/4;
b[4]='0'+adc_val2%10;
adc_val2=adc_val2/10;
b[3]='0'+adc_val2%10;
adc_val2=adc_val2/10;
b[2]='0'+adc_val2%10;
adc_val2=adc_val2/10;
b[1]='0'+adc_val2%10;
adc_val2=adc_val2/10;
b[0]='0'+adc_val2%10;
b[5]='\n';
b[6]='\0';
sendstring(b);
}
__bis_SR_register(LPM0_bits + GIE); // Enter LPM0, interrupts enabled
}
void average(coord* data, coord* dummy, int c)
{
int i;
char buffer[20];
for(i = 0; i < STEPS; i++)
{
data[i].x = (data[i].x*c + dummy[i].x)/(c + 1);
data[i].y = (data[i].y*c + dummy[i].y)/(c + 1);
sprintf(buffer,"%d %d \n", data[i].x, data[i].y);
sendstring(buffer);
}
}
void send_welcome(int n) {
auto packet = enet_packet_create(NULL, MAXTRANS, ENET_PACKET_FLAG_RELIABLE);
u8 *start = packet->data;
u8 *p = start+2;
putint(p, SV_INITS2C);
putint(p, n);
putint(p, PROTOCOL_VERSION);
putint(p, smapname[0]);
sendstring(serverpassword, p);
putint(p, clients.size()>maxclients);
if (smapname[0]) {
putint(p, SV_MAPCHANGE);
sendstring(smapname.c_str(), p);
putint(p, mode);
putint(p, SV_ITEMLIST);
loopv(sents) if (sents[i].spawned) putint(p, i);
putint(p, -1);
}
*(u16 *)start = ENET_HOST_TO_NET_16(int(p-start));
enet_packet_resize(packet, p-start);
send(n, packet);
}
int main(void){
char xbeebuffer[100];
int adcSample;
/**Setup Xbee*/
PORTD.DIR = 0b00001000;
PORTF.DIR = 3;
/**Setup interrupts*/
PMIC.CTRL |= PMIC_LOLVLEX_bm | PMIC_MEDLVLEX_bm | PMIC_HILVLEX_bm |
PMIC_LOLVLEN_bm | PMIC_MEDLVLEN_bm | PMIC_HILVLEN_bm;
sei();
USART_InterruptDriver_Initialize(&xbee, &USARTD0, USART_DREINTLVL_LO_gc);
USART_Format_Set(xbee.usart, USART_CHSIZE_8BIT_gc, USART_PMODE_DISABLED_gc, false);
USART_RxdInterruptLevel_Set(xbee.usart, USART_RXCINTLVL_HI_gc);
USART_Baudrate_Set(&USARTD0, 12 , 0);
USART_Rx_Enable(xbee.usart);
USART_Tx_Enable(xbee.usart);
ADC_Ch_InputMode_and_Gain_Config(&ADC_BK.CH0, ADC_CH_INPUTMODE_DIFF_gc, ADC_DRIVER_CH_GAIN_NONE); // differential mode, no gain
ADC_Ch_InputMux_Config(&ADC_BK.CH0, pin, ADC_CH_MUXNEG_PIN1_gc);
ADC_Reference_Config(&ADC_BK, ADC_REFSEL_VCC_gc); // use Vcc/1.6 as ADC reference
ADC_ConvMode_and_Resolution_Config(&ADC_BK, ADC_ConvMode_Signed, ADC_RESOLUTION_12BIT_gc);
ADC_Prescaler_Config(&ADC_BK, ADC_PRESCALER_DIV32_gc);
while(1){
if(readdata){
readdata = 0;
if(input == 'r'){
adc_start_conversion(&ADCA, ADC_CH0);
adc_wait_for_interrupt_flag(&ADCA, ADC_CH0);
adcSample = adcch_get_signed_result(&ADCA, 0);
sprintf(xbeebuffer, " %d\n\r", adcSample);
sendstring(&xbee, xbeebuffer);
}
}
}
}
int main(int argc, char** argv)
{
int serpid;
serpid = 0;
if(argc >= 3)
{
if((serpid = atoi(argv[1])) > 0)
{
my_str("--CLIPID-->");
my_int(getpid());
my_str("<--\n");
signal(SIGUSR1, ack);
signal(SIGALRM, omg);
sendpid(serpid, getpid());
if(!gl_ack)
{
alarm(5);
pause();
}
alarm(0);
gl_ack = 0;
my_str("--SENDING STRING-->");
my_str("<--\n");
sendstring(serpid, my_vect2str(&argv[2]));
gl_ack = 0;
my_str("--FINISHED SENDING STRING-->");
my_str("<--\n");
}
}
return 0;
}
int main(void){
char irinput;
char xbeebuffer[100];
PORTA.DIR = 0;
PORTB.DIR = 0b00000101;
/**Setup Xbee*/
PORTE.DIR = 0b00001000;
PORTF.DIR = 3;
USART_InterruptDriver_Initialize(&xbee, &USARTE0, USART_DREINTLVL_LO_gc);
USART_Format_Set(xbee.usart, USART_CHSIZE_8BIT_gc, USART_PMODE_DISABLED_gc, false);
USART_RxdInterruptLevel_Set(xbee.usart, USART_RXCINTLVL_HI_gc);
USART_Baudrate_Set(&USARTE0, 12 , 0);
USART_Rx_Enable(xbee.usart);
USART_Tx_Enable(xbee.usart);
while(1){
if(readdata){
readdata = 0;
sendchar(&xbee, &input);
if(input == 'r'){
PORTB.OUT |= 0x01;
_delay_ms(1);
while(PORTB.IN & 0x02);
PORTB.OUT &= ~0x01;
PORTB.OUT &= ~0x04;
_delay_ms(1);
PORTB.OUT |= 0x04;
irinput = PORTA.IN;
sprintf(xbeebuffer, " %d\n\r", irinput);
sendstring(&xbee, xbeebuffer);
}
}
}
}
int main(void){
enum states{running, stopped} state = stopped;
/**Move cmd vars*/
short int rise = 0;
short int rotate = 0;
short int forward = 0;
short int tilt = 0;
short int motorr = 0;
short int motorl = 0;
short int servor = 0;
short int servol = 0;
int i;
char xbeebuffer[100];
uint8_t accelsetupbuffer1[3] = {0x2C, 0b00001100, 0x08};
uint8_t accelsetupbuffer2[3] = {0x31, 0x00};
uint8_t accelstartbyte = 0x30;
uint8_t rollsetupbuffer1[4] = {0x15, 0x04, 0x19, 0x11};
uint8_t rollsetupbuffer2[] = {0x3E, 0b00000001};
uint8_t rollstartbyte = 0x1A;
char rollcash[3] = {0,0,0};
int accelcash[3] = {0,0,0};
//Pulse width modulation setup for servos, port D
TCD1.CTRLA = TC_CLKSEL_DIV1_gc;
TCD1.CTRLB = TC_WGMODE_SS_gc | TC0_CCAEN_bm |TC0_CCBEN_bm;
TCD1.PER = 40000;
TCC1.CTRLA = TC_CLKSEL_DIV1_gc;
TCC1.CTRLB = TC_WGMODE_SS_gc | TC0_CCAEN_bm |TC0_CCBEN_bm;
TCC1.PER = 40000;
TCC0.CTRLA = TC_CLKSEL_DIV1_gc;
TCC0.CTRLB = TC_WGMODE_SS_gc;
TCC0.PER = 40000;
/**Setup interrupts*/
PMIC.CTRL |= PMIC_LOLVLEX_bm | PMIC_MEDLVLEX_bm | PMIC_HILVLEX_bm |
PMIC_LOLVLEN_bm | PMIC_MEDLVLEN_bm | PMIC_HILVLEN_bm;
sei();
//Setup IMU
PORTD.DIR = 0x30;
PORTC.DIR = 0b00111100;
PORTC.OUT = 0b00001000;
TWI_MasterInit(&imu, &TWIC, TWI_MASTER_INTLVL_HI_gc, TWI_BAUDSETTING);
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ACCEL, accelsetupbuffer1, 3, 0);
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ACCEL, accelsetupbuffer2, 2, 0);
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ROLL, rollsetupbuffer1, 4, 0);
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ROLL, rollsetupbuffer2, 2, 0);
while(imu.status != TWIM_STATUS_READY);
TWIC.MASTER.CTRLB |= 0x0C;
/**Setup Xbee*/
PORTE.DIR = 0b00001000;
PORTF.DIR = 3;
USART_InterruptDriver_Initialize(&xbee, &USARTE0, USART_DREINTLVL_LO_gc);
USART_Format_Set(xbee.usart, USART_CHSIZE_8BIT_gc, USART_PMODE_DISABLED_gc, false);
USART_RxdInterruptLevel_Set(xbee.usart, USART_RXCINTLVL_HI_gc);
USART_Baudrate_Set(&USARTE0, 12 , 0);
USART_Rx_Enable(xbee.usart);
USART_Tx_Enable(xbee.usart);
while(1){
if(readdata){
readdata = 0;
sendchar(&xbee, input);
if(input == 'r'){
state = running;
}
else if(input == 's'){
state = stopped;
sprintf(xbeebuffer, "rise %4d tilt %4d rot %4d for %4d \n\r", rise, tilt, rotate, forward);
sendstring(&xbee, xbeebuffer);
}
else if(input == 'u'){
rise += 25;
}
else if(input == 'd'){
rise -= 25;
}
else if(input == 'c'){
rotate += 10;
}
else if(input == 'x'){
rotate -= 10;
}
else if(input == 'a'){
tilt += 10;
}
else if(input == 'e'){
tilt -= 10;
}
else if(input == 't'){
forward += 10;
}
else if(input == 'b'){
forward -= 10;
}
}
switch(state){
case stopped:
TCD1.CCA = 2000;
TCC1.CCA = SERVOLINI;
TCD1.CCB = 2000;
TCC1.CCB = SERVORINI;
break;
case running:
if(TCC0.INTFLAGS & 0x01){
TCC0.INTFLAGS = 0x01;
do{
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ROLL, &rollstartbyte, 1, 10);
PORTF.OUT = 2;
while(imu.result == TWIM_RESULT_UNKNOWN);
}while(!(imu.readData[0] & 0x01));
for(i = 0; i < 5; i += 2){
rollcash[i/2] += ((char)(imu.readData[i + 3]));
}
do{
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ACCEL, &accelstartbyte, 1, 10);
PORTF.OUT = 3;
while(imu.result == TWIM_RESULT_UNKNOWN);
PORTF.OUT = 1;
}while(!(imu.readData[0] & 0x80));
for(i = 0; i < 5; i += 2){
if(imu.readData[i + 3] & 0x80){
accelcash[i/2] -= 256 * (~imu.readData[i + 3] + 1);
accelcash[i/2] -= ~imu.readData[i + 2] + 1;
}
else{
accelcash[i/2] += 256 * imu.readData[i + 3];
accelcash[i/2] += imu.readData[i + 2];
}
}
PORTF.OUT = 0;
}
for(i = 0; i < 3; i ++){
accelcash[i] /= DAMPENACCEL;
rollcash[i] /= DAMPENROLL;
}
ValueFunk(accelcash[0],accelcash[1],accelcash[2],rollcash[0],rollcash[1],rollcash[2],&servol,&servor,&motorl,&motorr);
while(TCD1.CNT < 4000);
TCD1.CCA = motorl + rise - tilt;
TCD1.CCB = motorr + rise + tilt;
/*
while(TCC1.CNT < 4000);
TCC1.CCA = servol + rotate + forward;
TCC1.CCB = servor - rotate + forward;
*/
sprintf(xbeebuffer, " X%4d x%4d R%4d L%4d\n\r", rollcash[1], accelcash[0],motorr, motorl);
sendstring(&xbee, xbeebuffer);
for(i = 0; i < 3; i ++){
accelcash[i] *= INTEGRATEACCEL;
rollcash[i] *= INTEGRATEROLL;
}
break;
}
}
return 0;
}
int main(int argc, char **argv){
//joystick stick;
socketpair(AF_UNIX, SOCK_STREAM, 0, sv);
if(fork()){
printf("Read Thread Alive\n");
ros::init(argc, argv, "read_byte_client");
ros::NodeHandle r;
ros::ServiceClient rclient = r.serviceClient<rxtxserver::Read>("read_byte");
rxtxserver::Read data;
FILE * intercomm = fopen("intercomm", "w");
while(1){
printf("Read Client Attempting Read");
if(rclient.call(data)){
fputc(data.response.outData, intercomm);
fflush(intercomm);
}
usleep(1000);
}
return 1;
}
else{
printf("Write Thread Alive\n");
ros::init(argc, argv, "send_byte_client");
ros::NodeHandle n;
ros::ServiceClient client = n.serviceClient<rxtxserver::Byte>("send_byte");
FILE * intercomm2 = fopen("intercomm", "r");
time_t start,now;
char string[11];
string[0] = ' ';
string[1] = '0';
string[2] = 'a';
string[9] = 's';
string[10] = 0;
//int i;
char buf;
while(1){
/*
ros::spinOnce();
for(i = 3; i < 8; i ++){
string[i] = char(stick.axes[i-3]);
}
string[8] = 20;
for(i = 0; i < 11; i ++){
if(stick.button[i]){
string[8] = i;
}
}
if(string[8] == 0){
string[8] = 20;
}
*/
/*
sendstring(string);
for(i = 0;i<10;i ++){
usleep(4000);
if(rclient.call(data)){
printf("%c", data.response.outData);
}
}
*/
printf("Write Thread Sending Packet\n");
sendstring(string, &client);
time(&start);
do{
usleep(1000);
printf("checking data\n");
if(fscanf(intercomm2, "%c", buf) == 1){
printf("%c\n", buf);
}
time(&now);
}while(difftime(now, start) < 0.04);
}
printf("I'm Stopping for some reason\n");
return 1;
}
}
int main(void){
ADMUX = 0b01000000;
ADCSRA = 0x80;
char buffer0;
char buffer1;
char xbeebuffer[20];
char history0[10];
char history1[10];
char data;
int i = 0;
int j;
char slope0 = 0;
char slope1 = 0;
for(j = 0; j < 10; j ++){
history0[j] = 0;
history1[j] = 0;
}
UCSR0A = 0x02;
UCSR0B = 0x18;
UCSR0C = 0x06;
UBRR0L = 12;
DDRD = 0b10110010;
PORTD = 0b00110000;
while(1){
if(UCSR0A & 0x80){
data = UDR0;
//sendchar(data);
if(data == 'r'){
//PORTD ^= 0x20;
buffer0 = readadc(0)/10;
buffer1 = readadc(1)/10;
history0[i] = buffer0;
history1[i] = buffer1;
i ++;
if(i == 10){
i = 0;
}
for(j = 0; j < 9; j ++){
slope0 += (history0[i] - history0[i + 1])/2;
slope1 += (history1[i] - history1[i + 1])/2;
}
//sprintf(xbeebuffer, "%4d %4d %4d %4d\n\r", slope0, buffer0, slope1, buffer1);
//sprintf(xbeebuffer, " %d \n\r", buffer);
xbeebuffer[0] = 'r';
xbeebuffer[1] = ' ';
xbeebuffer[2] = buffer0;
xbeebuffer[3] = buffer1;
xbeebuffer[4] = 0;
sendstring(xbeebuffer);
slope0 = 0;
slope1 = 0;
}
}
}
return 0;
}
int
getvisrchstr(void)
{
char *sbuf = halloc(80);
int sptr = 1, ret = 0, ssbuf = 80;
int cmd;
int *obindtab = bindtab;
if (visrchstr) {
zsfree(visrchstr);
visrchstr = NULL;
}
clearlist = 1;
statusline = sbuf;
sbuf[0] = (visrchsense == -1) ? '?' : '/';
bindtab = mainbindtab;
while (sptr) {
sbuf[sptr] = '_';
statusll = sptr + 1;
refresh();
if ((cmd = getkeycmd()) < 0 || cmd == z_sendbreak) {
ret = 0;
break;
}
if(cmd == z_magicspace) {
c = ' ';
cmd = z_selfinsert;
}
switch(cmd) {
case z_redisplay:
redisplay();
break;
case z_clearscreen:
clearscreen();
break;
case z_acceptline:
case z_vicmdmode:
sbuf[sptr] = 0;
visrchstr = metafy(sbuf + 1, sptr - 1, META_DUP);
ret = 1;
sptr = 0;
break;
case z_backwarddeletechar:
case z_vibackwarddeletechar:
sptr--;
break;
case z_backwardkillword:
case z_vibackwardkillword:
while(sptr != 1 && iblank(sbuf[sptr - 1]))
sptr--;
if(iident(sbuf[sptr - 1]))
while(sptr != 1 && iident(sbuf[sptr - 1]))
sptr--;
else
while(sptr != 1 && !iident(sbuf[sptr - 1]) && !iblank(sbuf[sptr - 1]))
sptr--;
break;
case z_sendstring:
sendstring();
break;
case z_viquotedinsert:
sbuf[sptr] = '^';
refresh();
/* fall through */
case z_quotedinsert:
if ((c = getkey(0)) == EOF) {
feep();
break;
}
goto ins;
case z_selfinsertunmeta:
c &= 0x7f;
if(c == '\r')
c = '\n';
case z_selfinsert:
ins:
if(sptr == ssbuf - 1) {
char *newbuf = halloc(ssbuf *= 2);
strcpy(newbuf, sbuf);
statusline = sbuf = newbuf;
}
sbuf[sptr++] = c;
break;
default:
feep();
}
}
statusline = NULL;
bindtab = obindtab;
return ret;
}
void
doisearch(int dir)
{
char *s, *ibuf = halloc(80), *sbuf = ibuf + FIRST_SEARCH_CHAR;
int sbptr = 0, top_spot = 0, pos, sibuf = 80;
int nomatch = 0, skip_line = 0, skip_pos = 0;
int odir = dir, sens = zmult == 1 ? 3 : 1;
int hl = histline;
int cmd;
int *obindtab = bindtab;
static char *previous_search = NULL;
static int previous_search_len = 0;
clearlist = 1;
strcpy(ibuf, ISEARCH_PROMPT);
memcpy(ibuf + NORM_PROMPT_POS, (dir == 1) ? "fwd" : "bck", 3);
remember_edits();
s = zle_get_event(hl);
bindtab = mainbindtab;
pos = metalen(s, cs);
for (;;) {
/* Remember the current values in case search fails (doesn't push). */
set_isrch_spot(top_spot, hl, pos, cs, sbptr, dir, nomatch);
if (sbptr == 1 && sbuf[0] == '^') {
cs = 0;
nomatch = 0;
statusline = ibuf + NORM_PROMPT_POS;
} else if (sbptr > 0) {
char *last_line = s;
for (;;) {
char *t;
if (skip_pos) {
if (dir < 0) {
if (pos == 0)
skip_line = 1;
else
pos -= 1 + (pos != 1 && s[pos-2] == Meta);
} else if (sbuf[0] != '^') {
if (pos >= strlen(s+1))
skip_line = 1;
else
pos += 1 + (s[pos] == Meta);
} else
skip_line = 1;
skip_pos = 0;
}
if (!skip_line && ((sbuf[0] == '^') ?
(t = metadiffer(s, sbuf + 1, sbptr - 1) < sens ? s : NULL) :
(t = hstrnstr(s, pos, sbuf, sbptr, dir, sens)))) {
zle_goto_hist(hl);
pos = t - s;
cs = ztrsub(t, s) + (dir == 1? sbptr - (sbuf[0]=='^') : 0);
nomatch = 0;
statusline = ibuf + NORM_PROMPT_POS;
break;
}
hl += dir;
if (!(s = zle_get_event(hl))) {
if (sbptr == (int)isrch_spots[top_spot-1].len
&& (isrch_spots[top_spot-1].flags & ISS_FAILING))
top_spot--;
get_isrch_spot(top_spot, &hl, &pos, &cs, &sbptr,
&dir, &nomatch);
if (!nomatch) {
feep();
nomatch = 1;
}
s = last_line;
skip_line = 0;
statusline = ibuf;
break;
}
pos = dir == 1? 0 : strlen(s);
skip_line = !strcmp(last_line, s);
}
} else {
top_spot = 0;
nomatch = 0;
statusline = ibuf + NORM_PROMPT_POS;
}
sbuf[sbptr] = '_';
statusll = sbuf - statusline + sbptr + 1;
ref:
refresh();
if ((cmd = getkeycmd()) < 0 || cmd == z_sendbreak) {
int i;
get_isrch_spot(0, &hl, &pos, &i, &sbptr, &dir, &nomatch);
s = zle_get_event(hl);
zle_goto_hist(hl);
cs = i;
break;
}
switch (cmd) {
case z_clearscreen:
clearscreen();
goto ref;
case z_redisplay:
redisplay();
goto ref;
case z_vicmdmode:
bindtab = (bindtab == mainbindtab) ? altbindtab : mainbindtab;
goto ref;
case z_vibackwarddeletechar:
case z_backwarddeletechar:
if (top_spot)
get_isrch_spot(--top_spot, &hl, &pos, &cs, &sbptr,
&dir, &nomatch);
else
feep();
if (nomatch) {
statusline = ibuf;
skip_pos = 1;
}
s = zle_get_event(hl);
if (nomatch || !sbptr || (sbptr == 1 && sbuf[0] == '^')) {
int i = cs;
zle_goto_hist(hl);
cs = i;
}
memcpy(ibuf + NORM_PROMPT_POS, (dir == 1) ? "fwd" : "bck", 3);
continue;
case z_acceptandhold:
acceptandhold();
goto brk;
case z_acceptandinfernexthistory:
acceptandinfernexthistory();
goto brk;
case z_acceptlineanddownhistory:
acceptlineanddownhistory();
goto brk;
case z_acceptline:
acceptline();
goto brk;
case z_historyincrementalsearchbackward:
set_isrch_spot(top_spot++, hl, pos, cs, sbptr, dir, nomatch);
if (dir != -1)
dir = -1;
else
skip_pos = 1;
goto rpt;
case z_historyincrementalsearchforward:
set_isrch_spot(top_spot++, hl, pos, cs, sbptr, dir, nomatch);
if (dir != 1)
dir = 1;
else
skip_pos = 1;
goto rpt;
case z_virevrepeatsearch:
set_isrch_spot(top_spot++, hl, pos, cs, sbptr, dir, nomatch);
dir = -odir;
skip_pos = 1;
goto rpt;
case z_virepeatsearch:
set_isrch_spot(top_spot++, hl, pos, cs, sbptr, dir, nomatch);
dir = odir;
skip_pos = 1;
rpt:
if (!sbptr && previous_search_len) {
if (previous_search_len > sibuf - FIRST_SEARCH_CHAR - 2) {
ibuf = hrealloc(ibuf, sibuf, sibuf + previous_search_len);
sbuf = ibuf + FIRST_SEARCH_CHAR;
sibuf += previous_search_len;
}
memcpy(sbuf, previous_search, sbptr = previous_search_len);
}
memcpy(ibuf + NORM_PROMPT_POS, (dir == 1) ? "fwd" : "bck", 3);
continue;
case z_sendstring:
sendstring();
goto ref;
case z_viquotedinsert:
sbuf[sbptr] = '^';
refresh();
case z_quotedinsert:
if ((c = getkey(0)) == EOF) {
feep();
continue;
}
goto ins;
default:
if(cmd == z_selfinsertunmeta) {
c &= 0x7f;
if(c == '\r')
c = '\n';
} else if (cmd == z_magicspace)
c = ' ';
else if (cmd != z_selfinsert) {
ungetkeycmd();
if (cmd == z_sendbreak)
sbptr = 0;
goto brk;
}
ins:
if (sbptr == PATH_MAX) {
feep();
break;
}
set_isrch_spot(top_spot++, hl, pos, cs, sbptr, dir, nomatch);
if (sbptr == sibuf - FIRST_SEARCH_CHAR - 2) {
ibuf = hrealloc(ibuf, sibuf, sibuf * 2);
sbuf = ibuf + FIRST_SEARCH_CHAR;
sibuf *= 2;
}
sbuf[sbptr++] = c;
}
}
brk:
if (sbptr) {
zfree(previous_search, previous_search_len);
previous_search = zalloc(sbptr);
memcpy(previous_search, sbuf, previous_search_len = sbptr);
}
statusline = NULL;
bindtab = obindtab;
}
int main(void){
int integration[3] = {0,0,0};
char lostsignalcnt = 0;
int pry[] = {0,0,0};
int paceCounter = 0;
int pidValues13[3] = {6,20,24};
int pidValuesDen13[3] = {16,1,1};
int pidValues24[3] = {6,20,24};
int pidValuesDen24[3] = {16,1,1};
char pidRotUp[3] = {0,0,20};
char pidRotDenUp[3] = {42,1,1};
char pidRotDown[3] = {9,0,20};
char pidRotDenDown[3] = {42,1,1};
char pidRot[] = {5,0,20};
int throttledif = 0;
int throttleavr = 0;
/*counting var, for for loops*/
int i;
/*Start memory location for Accel and Gyro reads, should be moved
to gyro and accel read functions*/
uint8_t accelstartbyte = 0x30;
uint8_t gyrostartbyte = 0x1A;
/*Joystick Axis buffer
[0] - X axis tilt
[1] - Y axis tilt
[2] - Throttle
[3] - Rotation about Z axis
*/
int joyaxis[] = {0,0,0,0,0};
char joyin[] = {0,0,0,0,0};
int joytrim[] = {0,0,0,0,0};
int joydif[] = {0,0};
int joyavr[] = {0,0};
int motorSpeeds[4];
/*Var to allow increase in motor speed nonrelative to the throttle
during flight*/
int motorup = 0;
/*Vars for new input raw data (cache) and filtered data (int) from
imu*/
int gyrocache[3] = {0,0,0};
int accelcache[3] = {0,0,0};
int magcache[3] = {0,0,0};
int magfacing = 0;
int roterr = 0;
int target[] = {0,0,0};
int accelint[] = {0, 0, 0};
int gyroint[] = {0, 0, 0};
int gyrocounter[] = {0,0,0};
/*Standard values for accel and gyro (when level), set during offset*/
int accelnorm[3] = {28,-20,468};
char gyronorm[3] = {16,42,0};
/*Buffer for sending data through the xbee*/
char xbeebuffer[100];
CLK.CTRL = 0b00000011;
CLK.PSCTRL = 0b00010100;
/*Initialize PORTD to output on pins 0-3 from Timer counter pwm at
50Hz*/
PORTD.DIR = 0x2F;
TCD0.CTRLA = TC_CLKSEL_DIV1_gc;
TCD0.CTRLB = TC_WGMODE_SS_gc | TC0_CCCEN_bm | TC0_CCAEN_bm |TC0_CCBEN_bm | TC0_CCDEN_bm;
TCD0.PER = 8000;
/*Initialize Timer counter C0 for pacing,RATE Hz*/
TCC0.CTRLA = TC_CLKSEL_DIV1_gc;
TCC0.CTRLB = TC_WGMODE_SS_gc;
TCC0.PER = 2000000 / RATE;
/*Set on board LED pins to output*/
PORTF.DIR = 0x03;
/*Set PORTC to power IMU, PIN 3 3.3V, pin 2 ground*/
PORTC.DIR = 0b00001100;
PORTC.OUT = 0b00001000;
/*Enable global interrupts of all priority levels, should be made
more relevant*/
PMIC.CTRL |= PMIC_LOLVLEX_bm | PMIC_MEDLVLEX_bm | PMIC_HILVLEX_bm |
PMIC_LOLVLEN_bm | PMIC_MEDLVLEN_bm | PMIC_HILVLEN_bm;
sei();
/*Set pwm duty cycle to stop motors, stop them from beeping
annoyingly*/
TCD0.CCA = 2000;
TCD0.CCB = 2000;
TCD0.CCC = 2000;
TCD0.CCD = 2000;
/*Set Xbee Uart transmit pin 3 to output*/
PORTE.DIR = 0x08;
/*Initialize USARTE0 as the module used by the Xbee*/
uartInitiate(&xbee, &USARTE0);
/*Initialize USARTE1 as the module used by atmega328p */
uartInitiate(&atmega328p, &USARTE1);
/*Initialize imu to use Two wire interface on portC*/
twiInitiate(&imu, &TWIC);
itg3200Init(&imu, RATE);
adxl345Init(&imu);
lsm303dlhInit(&imu);
/*Send string to indicate startup, python doesn't like return carriage
(/r) character*/
sprintf(xbeebuffer, "starting\n");
sendstring(&xbee, xbeebuffer);
/*Start of flight control state machine loop*/
while(1){
/*Check for new packet from atmega328p*/
if(IRDataAvailable)
{
sprintf(xbeebuffer, "Altitude: %d\n", (irdata[1]*256) + irdata[2]);
sendstring(&xbee, xbeebuffer);
}
/*Check for new packet from xbee each time*/
if(readdata){
readdata = 0;
lostsignalcnt = 0;
/*For Joystick packet reads*/
joytrim[2] = 0;
for(i = 0; i < 5; i++){
joyin[i] = -input[3 + i] + 126;
joyaxis[i] = joyin[i];
joyaxis[i] += joytrim[i];
}
throttleavr = ((throttleavr) + (joyaxis[2]))/2;
throttledif = joyaxis[2] - throttleavr;
joyaxis[2] += throttledif * THROTTLEJOYDIF;
for(i = 0; i < 2; i++){
joyavr[i] = (joyavr[i] + joyaxis[i])/2;
joydif[i] = joyaxis[i] - joyavr[i];
}
joyaxis[1] += joydif[1] * PRJOYDIF13;
joyaxis[0] += joydif[0] * PRJOYDIF24;
/*
yawavr = ((yawavr) + joyaxis[3])/2;
yawdif = joyaxis[3] - yawavr;
joyaxis[3] += yawdif * YAWJOYDIF;
*/
//Input 7 is the button buffer
if(input[8] == 4){
state = stopped;
//sprintf(xbeebuffer, "stopped %d\n", input[7]);
sprintf(xbeebuffer, "%4d %4d %4d %4d\n", joyaxis[0], joyaxis[1], joyaxis[2], joyaxis[3]);
sendstring(&xbee, xbeebuffer);
}
else if(input[8] == 0){
joytrim[0] += joyin[0];
joytrim[1] += joyin[1];
joytrim[3] += joyin[3];
}
else if(input[8] == 1){
state = running;
sprintf(xbeebuffer, "running %d\n", input[7]);
sendstring(&xbee, xbeebuffer);
}
else if(input[8] == 10){
state = offset;
}
else if(input[8] == 5){
//motorup += 5;
pidRot[2] ++;
sprintf(xbeebuffer, "D up %d\n", pidRot[2]);
//pidRot[2] ++;
//sprintf(xbeebuffer, "D up %d\n", pidValues24[2]);
sendstring(&xbee, xbeebuffer);
}
else if(input[8] == 6){
pidRot[2] --;
sprintf(xbeebuffer, "D down %d\n", pidRot[2]);
//pidRot[2] --;
//sprintf(xbeebuffer, "D down %d\n", pidValues24[2]);
sendstring(&xbee, xbeebuffer);
//motorup -= 5;
}
else if(input[8] == 7){
pidRot[0] ++;
sprintf(xbeebuffer, "P up %d\n", pidRot[0]);
//pidRot[0] ++;
//sprintf(xbeebuffer, "P up %d\n", pidValues24[0]);
sendstring(&xbee, xbeebuffer);
}
else if(input[8] == 8){
pidRot[0] --;
sprintf(xbeebuffer, "P down %d\n", pidRot[0]);
//pidRot[0] --;
//sprintf(xbeebuffer, "P down %d\n", pidValues24[0]);
sendstring(&xbee, xbeebuffer);
/*
getmag(magcache, &imu);
sprintf(xbeebuffer, "%4d %4d %4d\n", magcache[0], magcache[1], magcache[2]);
sendstring(&xbee, xbeebuffer);
*/
}
else if(input[8] == 2){
sprintf(xbeebuffer, "descending\n");
sendstring(&xbee, xbeebuffer);
motorup = -50;
}
xbeecounter = 0;
for(i=0;i<3;i++){
pidRotUp[i] = pidRot[i] * 3/4;
pidRotDown[i] = pidRot[i] * 1;
}
if(state == running){
//sprintf(xbeebuffer, "%d %d\n", joyaxis[2], throttledif);
//sprintf(xbeebuffer, "%d %d %d \n", joyaxis[0], joyaxis[1], joyaxis[3]);
//sprintf(xbeebuffer, "%4d %4d %4d\n", pry[0], pry[1], pry[2]);
//sprintf(xbeebuffer, "%3d %3d\n", gyroint[2], joyaxis[3]);
//sprintf(xbeebuffer, "%4d %4d %4d %4d\n", motorSpeeds[0], motorSpeeds[1], motorSpeeds[2], motorSpeeds[3]);
//sprintf(xbeebuffer, "%4d %4d %4d\n", accelint[0], accelint[1], accelint[2]);
//sprintf(xbeebuffer, "%4d %4d %4d\n", magcache[0], magcache[1], magcache[2]);
sprintf(xbeebuffer, "%4d\n", roterr);
sendstring(&xbee, xbeebuffer);
}
}
switch(state){
/*Stopped state keeps motors stopped but not beeping*/
case stopped:
TCD0.CCA = 2000;
TCD0.CCB = 2000;
TCD0.CCC = 2000;
TCD0.CCD = 2000;
break;
/*Offset gets standard value for gyro's and accel's*/
case offset:
getgyro(gyrocache, &imu, &gyrostartbyte);
getaccel(accelcache, &imu, &accelstartbyte);
getmag(magcache, &imu);
target[2] = arctan2(magcache[0], magcache[1]);
for(i = 0; i < 3; i ++){
gyronorm[i] = gyrocache[i];
//accelnorm[i] = accelcache[i];
accelcache[i] = 0;
gyrocache[i] = 0;
}
sprintf(xbeebuffer, "offset %d %d %d %d %d %d\n", gyronorm[0], gyronorm[1], gyronorm[2], accelnorm[0], accelnorm[1], accelnorm[2]);
sendstring(&xbee, xbeebuffer);
state = stopped;
break;
case running:
/*Ensure loop doesn't go faster than 50Hz*/
while(!(TCC0.INTFLAGS & 0x01));
TCC0.INTFLAGS = 0x01;
/*Get gyro data
substract stationary offset
filter for stability
*/
getgyro(gyrocache, &imu, &gyrostartbyte);
for(i = 0; i < 3; i ++){
gyrocache[i] -= gyronorm[i];
if((gyrocache[i] <= 1) && (gyrocache[i] >= -1)){
gyrocache[i] = 0;
}
gyrocounter[i] += gyrocache[i];
}
for(i = 0; i < 3; i += 2){
gyroint[i] = gyrocounter[i]/DEGREE;
gyrocounter[i] %= DEGREE;
pry[i] += gyroint[i];
if(pry[i] > PI){
pry[i] = -PI + (pry[i] - PI);
}
else if(pry[i] < -PI){
pry[i] = PI + (pry[i] + PI);
}
}
gyroint[1] = -(gyrocounter[1]/DEGREE);
gyrocounter[1] %= DEGREE;
pry[1] += gyroint[1];
paceCounter ++;
//Slower Operations at 50Hz
if(paceCounter == (RATE / 20)){
paceCounter = 0;
lostsignalcnt ++;
sendbyte(&atmega328p, 'r'); //ask for IR data
getaccel(accelcache, &imu, &accelstartbyte);
getmag(magcache, &imu);
magfacing = arctan2(magcache[0], magcache[1]);
if((4900 - abs(pry[2]) - abs(target[2])) < abs(pry[2] - target[2])){
if(target > 0){
roterr = 4900 - abs(target[2]) - abs(pry[2]);
}
else{
roterr = -(4900 - abs(target[2]) - abs(pry[2]));
}
}
else{
roterr = target[2] - pry[2];
}
for(i = 0; i < 3; i ++){
accelcache[i] -= accelnorm[i];
/*
if(accelcache[i] > (accelint[i] + 40)){
accelcache[i] = accelint[i] + 40;
}
else if(accelcache[i] < (accelint[i] - 40)){
accelcache[i] = accelint[i] - 40;
}
*/
}
accelint[0] = ((ACCELINT * accelint[0]) + ((24 - ACCELINT) * accelcache[0]))/24;
accelint[1] = ((ACCELINT * accelint[1]) + ((24 - ACCELINT) * accelcache[1]))/24;
if(accelint[1] > (pry[0] + 15)){
accelint[1] = pry[0] + 15;
}
else if(accelint[1] < (pry[0] - 15)){
accelint[1] = pry[0] - 15;
}
if(accelint[0] > (pry[1] + 15)){
accelint[0] = pry[1] + 15;
}
else if(accelint[0] < (pry[1] - 15)){
accelint[0] = pry[1] - 15;
}
pry[0] = ((AWEIGHT * accelint[1]) + (GWEIGHT * pry[0])) / (AWEIGHT + GWEIGHT);
pry[1] = ((AWEIGHT * accelint[0]) + (GWEIGHT * pry[1])) / (AWEIGHT + GWEIGHT);
/*reset cache values to 0, should be made unnecessary by modding gyro and
accel read functions*/
for(i = 0; i < 3; i ++){
accelcache[i] = 0;
}
}
if(gyroint[0] > 6){
gyroint[0] = 6;
}
else if(gyroint[0] < -6){
gyroint[0] = -6;
}
if(gyroint[1] > 6){
gyroint[1] = 6;
}
else if(gyroint[1] < -6){
gyroint[1] = -6;
}
motorSpeed(pry, integration ,gyroint, joyaxis, motorSpeeds, pidValues13, pidValues24, pidValuesDen13, pidValuesDen24);
yawCorrect(motorSpeeds, gyroint, &roterr,pidRotUp,pidRotDenUp,pidRotDown,pidRotDenDown);
if(lostsignalcnt > 10){
for(i = 0; i < 4; i ++){
motorSpeeds[i] -= 50;
}
}
while(!((TCD0.CNT > 5000) || (TCD0.CNT < 2500)));
TCD0.CCA = motorSpeeds[0] + motorup;// - motordif13;
TCD0.CCC = motorSpeeds[2] + motorup;// + motordif13;
TCD0.CCB = motorSpeeds[1] + motorup;// + motordif24;
TCD0.CCD = motorSpeeds[3] + motorup;// - motordif24;
PORTD.OUT ^= 0b00100000;
}
}
}
int main(void){
int integration[3] = {0,0,0};
int lostsignalcnt = 0;
char motordif24 = -15;
char motordif13 = 10;
uint8_t testchar = 0;
uint8_t testchar2 = 0;
int pry[] = {0,0,0};
int aVec[] = {0,0,0};
int paceCounter = 0;
int tilt = 0;
int pidValues[3] = {14,0,28};
int pidValuesDen[3] = {16,1,1};
/*counting var, for for loops*/
int i;
/*Start memory location for Accel and Gyro reads, should be moved
to gyro and accel read functions*/
uint8_t accelstartbyte = 0x30;
uint8_t gyrostartbyte = 0x1A;
/*Joystick Axis buffer
[0] - X axis tilt
[1] - Y axis tilt
[2] - Throttle
[3] - Rotation about Z axis
*/
int joyaxis[] = {0,0,0,0};
char joyin[] = {0,0,0,0};
int joytrim[] = {0,0,0,0};
int motorSpeeds[4];
/*Var to allow increase in motor speed nonrelative to the throttle
during flight*/
int motorup = 0;
/*Vars for new input raw data (cache) and filtered data (int) from
imu*/
int gyrocache[3] = {0,0,0};
int accelcache[3] = {0,0,0};
int accelint[] = {0, 0, 0};
int gyroint[] = {0, 0, 0};
int gyrocounter[] = {0,0,0};
/*Standard values for accel and gyro (when level), set during offset*/
int accelnorm[3] = {38,-30,470};
char gyronorm[3] = {16,42,0};
/*Buffer for sending data through the xbee*/
char xbeebuffer[100];
/*Initialize PORTD to output on pins 0-3 from Timer counter pwm at
50Hz*/
PORTD.DIR = 0x2F;
TCD0.CTRLA = TC_CLKSEL_DIV1_gc;
TCD0.CTRLB = TC_WGMODE_SS_gc | TC0_CCCEN_bm | TC0_CCAEN_bm |TC0_CCBEN_bm | TC0_CCDEN_bm;
TCD0.PER = 4000;
/*Initialize Timer counter C0 for pacing,RATE Hz*/
TCC0.CTRLA = TC_CLKSEL_DIV1_gc;
TCC0.CTRLB = TC_WGMODE_SS_gc;
TCC0.PER = 2000000 / RATE;
/*Set on board LED pins to output*/
PORTF.DIR = 0x03;
/*Set PORTC to power IMU, PIN 3 3.3V, pin 2 ground*/
PORTC.DIR = 0b00001100;
PORTC.OUT = 0b00001000;
/*Enable global interrupts of all priority levels, should be made
more relevant*/
PMIC.CTRL |= PMIC_LOLVLEX_bm | PMIC_MEDLVLEX_bm | PMIC_HILVLEX_bm |
PMIC_LOLVLEN_bm | PMIC_MEDLVLEN_bm | PMIC_HILVLEN_bm;
sei();
/*Set pwm duty cycle to stop motors, stop them from beeping
annoyingly*/
TCD0.CCA = 2000;
TCD0.CCB = 2000;
TCD0.CCC = 2000;
TCD0.CCD = 2000;
/*Set Xbee Uart transmit pin 3 to output*/
PORTE.DIR = 0x08;
/*Initialize USARTE0 as the module used by the Xbee*/
uartInitiate(&xbee, &USARTE0);
/*Initialize imu to use Two wire interface on portC*/
twiInitiate(&imu, &TWIC);
/*Send string to indicate startup, python doesn't like return carriage
(/r) character*/
sprintf(xbeebuffer, "starting\n");
sendstring(&xbee, xbeebuffer);
/*Start of flight control state machine loop*/
while(1){
/*Check for new packet from xbee each time*/
if(readdata){
readdata = 0;
lostsignalcnt = 0;
/*For non joystick control*/
/*
if (input[0] == 'r') {
state = running;
sprintf(xbeebuffer, "running\r\n");
sendstring(&xbee, xbeebuffer);
}
else if (input[0] == 's') {
state =stopped;
sprintf(xbeebuffer, "stopped\r\n");
sendstring(&xbee, xbeebuffer);
}
else if (input[0] == 'o'){
state = offset;
}
else if (input[0] == 'u'){
pidValues[2] ++;
sprintf(xbeebuffer, "%d/%d\n\r", pidValues[2], pidValuesDen[2]);
sendstring(&xbee, xbeebuffer);
}
else if (input[0] == 'j'){
pidValues[2] --;
sprintf(xbeebuffer, "%d/%d\n\r", pidValues[2], pidValuesDen[2]);
sendstring(&xbee, xbeebuffer);
}
else if (input[0] == 'i'){
pidValuesDen[2] ++;
sprintf(xbeebuffer, "%d/%d\n\r", pidValues[2], pidValuesDen[2]);
sendstring(&xbee, xbeebuffer);
}
else if (input[0] == 'k'){
pidValuesDen[2] --;
sprintf(xbeebuffer, "%d/%d\n\r", pidValues[2], pidValuesDen[2]);
sendstring(&xbee, xbeebuffer);
}
else if (input[0] == 'd'){
pidValues[0] ++;
sprintf(xbeebuffer, "%d/%d\n\r", pidValues[0], pidValuesDen[0]);
sendstring(&xbee, xbeebuffer);
}
else if (input[0] == 'c'){
pidValues[0] --;
sprintf(xbeebuffer, "%d/%d\n\r", pidValues[0], pidValuesDen[0]);
sendstring(&xbee, xbeebuffer);
}
else if (input[0] == 'f'){
pidValuesDen[0] ++;
sprintf(xbeebuffer, "%d/%d\n\r", pidValues[0], pidValuesDen[0]);
sendstring(&xbee, xbeebuffer);
}
else if (input[0] == 'v'){
pidValuesDen[0] --;
sprintf(xbeebuffer, "%d/%d\n\r", pidValues[0], pidValuesDen[0]);
sendstring(&xbee, xbeebuffer);
}
else if(input[0] == 'y'){
motorup += 25;
}
else if(input[0] == 'h'){
motorup -=25;
}
else if(input[0] == 't'){
motordif24 += 5;
}
else if(input[0] == 'g'){
motordif24 -= 5;
}
else if(input[0] == 'a'){
motordif13 += 5;
}
else if(input[0] == 'z'){
motordif13 -= 5;
}
else if(input[0] == '0'){
cli();
while(1){
TCD0.CCA = 2000;
TCD0.CCB = 2000;
TCD0.CCC = 2000;
TCD0.CCD = 2000;
}
}
*/
/*For Joystick packet reads*/
joytrim[2] = 0;
for(i = 0; i < 4; i ++){
joyin[i] = -input[3 + i] + 126;
joyaxis[i] = joyin[i];
joyaxis[i] += joytrim[i];
}
//Input 7 is the button buffer
if(input[7] == 3){
state = stopped;
sprintf(xbeebuffer, "stopped %d\n", input[7]);
//sprintf(xbeebuffer, "%4d %4d %4d %4d\n\r", motor1, motor2, motor3, motor4);
sprintf(xbeebuffer, "%4d %4d %4d %4d\n", joyaxis[0], joyaxis[1], joyaxis[2], joyaxis[3]);
sendstring(&xbee, xbeebuffer);
}
else if(input[7] == 0){
joytrim[0] += joyin[0];
joytrim[1] += joyin[1];
joytrim[3] += joyin[3];
}
else if(input[7] == 4){
state = running;
sprintf(xbeebuffer, "running %d\n", input[7]);
sendstring(&xbee, xbeebuffer);
}
else if(input[7] == 5){
state = offset;
}
else if(input[7] == 6){
motorup += 5;
sprintf(xbeebuffer, "up\n");
sendstring(&xbee, xbeebuffer);
}
else if(input[7] == 7){
sprintf(xbeebuffer, "down\n");
sendstring(&xbee, xbeebuffer);
motorup -= 5;
}
xbeecounter = 0;
}
switch(state){
/*Stopped state keeps motors stopped but not beeping*/
case stopped:
TCD0.CCA = 2000;
TCD0.CCB = 2000;
TCD0.CCC = 2000;
TCD0.CCD = 2000;
break;
/*Offset gets standard value for gyro's and accel's*/
case offset:
getgyro(gyrocache, &imu, &gyrostartbyte);
getaccel(accelcache, &imu, &accelstartbyte);
for(i = 0; i < 3; i ++){
gyronorm[i] = gyrocache[i];
accelnorm[i] = accelcache[i];
accelcache[i] = 0;
gyrocache[i] = 0;
}
sprintf(xbeebuffer, "offset %d %d %d %d %d %d\n", gyronorm[0], gyronorm[1], gyronorm[2], accelnorm[0], accelnorm[1], accelnorm[2]);
sendstring(&xbee, xbeebuffer);
state = stopped;
break;
case running:
/*Ensure loop doesn't go faster than 50Hz*/
while(!(TCC0.INTFLAGS & 0x01));
TCC0.INTFLAGS = 0x01;
/*Get gyro data
substract stationary offset
filter for stability
*/
getgyro(gyrocache, &imu, &gyrostartbyte);
for(i = 0; i < 3; i ++){
gyrocache[i] -= gyronorm[i];
if((gyrocache[i] <= 1) && (gyrocache[i] >= -1)){
gyrocache[i] = 0;
}
gyrocounter[i] += gyrocache[i];
}
for(i = 0; i < 3; i ++){
// gyroint[i] = ((GYROINT)* gyroint[i]);
// gyroint[i] += (20 - GYROINT) * gyrocounter[i]/DEGREE;
gyroint[i] = gyrocounter[i]/DEGREE;
gyrocounter[i] %= DEGREE;
//gyroint[i] /= 20;
pry[i] += gyroint[i];
}
paceCounter ++;
//Slower Operations at 50Hz
if(paceCounter == (RATE / 50)){
paceCounter = 0;
lostsignalcnt ++;
getaccel(accelcache, &imu, &accelstartbyte);
for(i = 0; i < 3; i ++){
accelcache[i] -= accelnorm[i];
}
accelint[0] = -((ACCELINT * accelint[0]) + ((20 - ACCELINT) * accelcache[0]))/20;
accelint[1] = ((ACCELINT * accelint[1]) + ((20 - ACCELINT) * accelcache[1]))/20;
if(accelint[1] > (pry[0] + 10)){
accelint[1] = pry[0] + 10;
}
else if(accelint[1] < (pry[0] - 10)){
accelint[1] = pry[0] - 10;
}
if(accelint[0] > (pry[1] + 10)){
accelint[0] = pry[1] + 10;
}
else if(accelint[0] < (pry[1] - 10)){
accelint[0] = pry[1] - 10;
}
pry[0] = ((AWEIGHT * accelint[1]) + (GWEIGHT * pry[0])) / (AWEIGHT + GWEIGHT);
pry[1] = ((AWEIGHT * accelint[0]) + (GWEIGHT * pry[1])) / (AWEIGHT + GWEIGHT);
//sprintf(xbeebuffer, "%4d %4d %4d\n\r", gyrocache[0], gyrocache[1], gyrocache[2]);
//sprintf(xbeebuffer, "%d %d %d\n\r", pry[0], pry[1], pry[2]);
//sprintf(xbeebuffer, "%d %d %d\n\r", accelint[0], accelint[1], accelint[2]);
//sprintf(xbeebuffer, "%d %d %d\n\r", currentMatrix[0], currentMatrix[1], currentMatrix[2]);
//sprintf(xbeebuffer, "%3d %3d %3d\n\r", gyroint[0], gyroint[1], gyroint[2]);
//sprintf(xbeebuffer, "%4d %4d %4d %4d\n\r", motorSpeeds[0], motorSpeeds[1], motorSpeeds[2], motorSpeeds[3]);
//sprintf(xbeebuffer, "%d\n\r", gyrocache[1]);
//sprintBinary(xbeebuffer, &gyrocache[0]);
//sendstring(&xbee, xbeebuffer);
/*reset cache values to 0, should be made unnecessary by modding gyro and
accel read functions*/
for(i = 0; i < 3; i ++){
accelcache[i] = 0;
}
}
/*Get accel data
substract accelnorm (should change that, won't work with dcm)
Filter
*/
motorSpeed(pry, integration ,gyroint, joyaxis, motorSpeeds, pidValues, pidValuesDen);
if(lostsignalcnt > 50){
for(i = 0; i < 4; i ++){
motorSpeeds[i] -= 50;
}
}
TCD0.CCA = motorSpeeds[0] + motorup;// - motordif13;
TCD0.CCC = motorSpeeds[2] + motorup;// + motordif13;
TCD0.CCB = motorSpeeds[1] + motorup;// + motordif24;
TCD0.CCD = motorSpeeds[3] + motorup;// - motordif24;
pry[2] = 0;
PORTD.OUT ^= 0b00100000;
/*
if((abs(pry[0]) > 80) || (abs(pry[1]) > 80)){
state = stopped;
}
*/
/*
for(i = 1; i < 10; i ++){
xbeebuffer[i] = currentMatrix[i - 1] + 62;
}
xbeebuffer[0] = 0xFB;
xbeebuffer[10] = 0xFF;
xbeebuffer[11] = 0;
*/
}
}
}
int main(void){
enum states{running, stopped} state = stopped;
char input;
int i;
char xbeebuffer[100];
uint8_t accelsetupbuffer1[3] = {0x2C, 0b00001100, 0x08};
uint8_t accelsetupbuffer2[3] = {0x31, 0x00};
uint8_t accelstartbyte = 0x30;
uint8_t rollsetupbuffer1[4] = {0x15, 0x04, 0x19, 0x11};
uint8_t rollsetupbuffer2[] = {0x3E, 0b00000001};
uint8_t rollstartbyte = 0x1A;
char rollcash[3] = {0,0,0};
int accelcash[3] = {0,0,0};
TCC0.CTRLA = TC_CLKSEL_DIV1_gc;
TCC0.CTRLB = TC_WGMODE_SS_gc;
TCC0.PER = 40000;
/**Setup interrupts*/
PMIC.CTRL |= PMIC_LOLVLEX_bm | PMIC_MEDLVLEX_bm | PMIC_HILVLEX_bm |
PMIC_LOLVLEN_bm | PMIC_MEDLVLEN_bm | PMIC_HILVLEN_bm;
sei();
//Setup IMU
PORTC.DIR = 0b00001100;
PORTC.OUT = 0b00001000;
TWI_MasterInit(&imu, &TWIC, TWI_MASTER_INTLVL_HI_gc, TWI_BAUDSETTING);
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ACCEL, accelsetupbuffer1, 3, 0);
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ACCEL, accelsetupbuffer2, 2, 0);
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ROLL, rollsetupbuffer1, 4, 0);
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ROLL, rollsetupbuffer2, 2, 0);
while(imu.status != TWIM_STATUS_READY);
/**Setup Xbee*/
PORTE.DIR = 0b00001000;
PORTF.DIR = 3;
USART_InterruptDriver_Initialize(&xbee, &USARTE0, USART_DREINTLVL_LO_gc);
USART_Format_Set(xbee.usart, USART_CHSIZE_8BIT_gc, USART_PMODE_DISABLED_gc, false);
USART_RxdInterruptLevel_Set(xbee.usart, USART_RXCINTLVL_HI_gc);
USART_Baudrate_Set(&USARTE0, 12 , 0);
USART_Rx_Enable(xbee.usart);
USART_Tx_Enable(xbee.usart);
while(1){
if(USART_RXBufferData_Available(&xbee)){
input = USART_RXBuffer_GetByte(&xbee);
sendchar(&xbee, input);
if(input == 'r'){
state = running;
}
else if(input == 's'){
PORTF.OUT ^= 0x02;
state = stopped;
}
}
switch(state){
case stopped:
break;
case running:
if(TCC0.INTFLAGS & 0x01){
for(i = 0; i < 3; i ++){
rollcash[i] = 0;
accelcash[i] = 0;
}
TCC0.INTFLAGS = 0x01;
do{
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ROLL, &rollstartbyte, 1, 10);
while(imu.result == TWIM_RESULT_UNKNOWN);
}while(!(imu.readData[0] & 0x01));
for(i = 0; i < 5; i += 2){
rollcash[i/2] += ((char)(imu.readData[i + 3]));
}
PORTF.OUT = 1;
do{
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ACCEL, &accelstartbyte, 1, 10);
while(imu.result == TWIM_RESULT_UNKNOWN);
}while(!(imu.readData[0] & 0x80));
for(i = 0; i < 5; i += 2){
if(imu.readData[i + 3] & 0x80){
accelcash[i/2] -= 256 * (~imu.readData[i + 3] + 1);
accelcash[i/2] -= ~imu.readData[i + 2] + 1;
}
else{
accelcash[i/2] += 256 * imu.readData[i + 3];
accelcash[i/2] += imu.readData[i + 2];
}
}
sprintf(xbeebuffer, "%d %d\n\r", rollcash[0], accelcash[0]);
sendstring(&xbee, xbeebuffer);
}
break;
}
}
return 0;
}
int handleInvalidEntryState(pcpuinfo currentcpuinfo,VMRegisters *vmregisters)
{
sendstring("Handling invalid entry state\n\r");
//fix interruptability state (common bug when emulating and fixing it won't cause a problem)
fixInterruptabilityState();
if (ISREALMODE(currentcpuinfo))
{
int result;
sendstring("Inside realmode. Trying to emulate instructions\n\r");
result=emulateRealMode(currentcpuinfo, vmregisters)==0;
sendstringf("emulateRealMode(...) returned %d\n\r",result);
if (result==0)
{
sendstring("emulation was handled properly\n");
return 0; //handled at least one instruction
}
else
sendstring("emulation was a total failure. Not one instruction got emulated. Trying to fix the state");
//emulateRealMode failed
if (ISREALMODE(currentcpuinfo)) //still realmode ? (most likely, but possible it isn't anymore once enough has been emulated)
{
Access_Rights reg_rmaccessrights,reg_traccessrights;
RFLAGS guestrflags;
DWORD gdtbase, idtbase;
guestrflags.value=0;
gdtbase=VirtualToPhysical(getGDTbase());
idtbase=VirtualToPhysical((UINT64)idttable32);
sendstring("Still in realmode, enabling VMx86 mode if not already in it and fixing possible other bugs\n\r");
//set taskregister to realmode tr
//set GDT and IDT to my own (so a 32-bit interrupt is possible)
vmwrite(vm_guest_gdtr_base, gdtbase);
vmwrite(vm_guest_gdt_limit, getGDTsize());
vmwrite(vm_guest_idtr_base, idtbase);
vmwrite(vm_guest_idt_limit, 256*8);
setupTSS8086();
vmwrite(vm_guest_tr_base,(UINT64)VirtualToPhysical((UINT64)VirtualMachineTSS_V8086)); //tr base
vmwrite(vm_guest_tr_limit,(ULONG)sizeof(TSS)+32+8192+1); //tr limit
vmwrite(vm_guest_tr,64); //the tss o
#ifdef DEBUG
UINT64 idtbase2, gdtbase2;
gdtbase2=vmread(vm_guest_gdtr_base);
idtbase2=vmread(vm_guest_idtr_base);
sendstringf("Set vm_guest_gdtr_base to %6 while I wanted to set it to %6\n\r",gdtbase2, gdtbase);
sendstringf("Set vm_guest_idtr_base to %6 while I wanted to set it to %6\n\r",idtbase2, idtbase);
#endif
reg_rmaccessrights.AccessRights=0;
reg_rmaccessrights.Segment_type=3;
reg_rmaccessrights.S=1;
reg_rmaccessrights.DPL=3;
reg_rmaccessrights.P=1;
reg_rmaccessrights.G=0;
reg_rmaccessrights.D_B=0;
reg_traccessrights.AccessRights=0;
reg_traccessrights.Segment_type=11; //11=32-bit 3=16-bit
reg_traccessrights.S=0;
reg_traccessrights.DPL=0;
reg_traccessrights.P=1;
reg_traccessrights.G=0;
reg_traccessrights.D_B=1;
vmwrite(vm_guest_es_access_rights,(ULONG)reg_rmaccessrights.AccessRights); //es access rights
vmwrite(vm_guest_cs_access_rights,(ULONG)reg_rmaccessrights.AccessRights); //cs access rights
vmwrite(vm_guest_ss_access_rights,(ULONG)reg_rmaccessrights.AccessRights); //ss access rights
vmwrite(vm_guest_ds_access_rights,(ULONG)reg_rmaccessrights.AccessRights); //ds access rights
vmwrite(vm_guest_fs_access_rights,(ULONG)reg_rmaccessrights.AccessRights); //fs access rights
vmwrite(vm_guest_gs_access_rights,(ULONG)reg_rmaccessrights.AccessRights); //gs access rights
vmwrite(vm_guest_ldtr_access_rights,(ULONG)(1<<16)); //ldtr access rights (bit 16 is unusable bit
vmwrite(vm_guest_tr_access_rights,(ULONG)reg_traccessrights.AccessRights); //tr access rights
vmwrite(vm_guest_es,(ULONG)vmread(vm_guest_es_base) >> 4); //es selector
vmwrite(vm_guest_cs,(ULONG)vmread(vm_guest_cs_base) >> 4); //cs selector
vmwrite(vm_guest_ss,(ULONG)vmread(vm_guest_ss_base) >> 4); //ss selector
vmwrite(vm_guest_ds,(ULONG)vmread(vm_guest_ds_base) >> 4); //ds selector
vmwrite(vm_guest_fs,(ULONG)vmread(vm_guest_fs_base) >> 4); //fs selector
vmwrite(vm_guest_gs,(ULONG)vmread(vm_guest_gs_base) >> 4); //gs selector
vmwrite(vm_guest_ldtr,(ULONG)0); //ldtr selector
vmwrite(vm_guest_tr,(ULONG)0); //tr selector
vmwrite(vm_guest_es_limit,(ULONG)0xffff); //es limit
vmwrite(vm_guest_cs_limit,(ULONG)0xffff); //cs limit
vmwrite(vm_guest_ss_limit,(ULONG)0xffff); //ss limit
vmwrite(vm_guest_ds_limit,(ULONG)0xffff); //ds limit
vmwrite(vm_guest_fs_limit,(ULONG)0xffff); //fs limit
vmwrite(vm_guest_gs_limit,(ULONG)0xffff); //gs limit
//make sure VM flag is set appropriatly for vm mode
guestrflags.value=vmread(vm_guest_rflags);
guestrflags.IOPL=3;
guestrflags.VM=1;
// pguesteflags->v
//currentcpuinfo->hasIF=pguesteflags->IF;
vmwrite(vm_guest_rflags,guestrflags.value);
return 0;
}
}
else
{
