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 {