void output_init() { pinMode(PWM_PIN,PWM_OUTPUT); /* 500/1000 = 0.5 duty cycle */ #if 1 pwmSetRange(1000) ; pwmWrite(PWM_PIN, 500); #else pwmSetRange(2600) ; pwmWrite(PWM_PIN, 800); #endif pwmSetClock(252); }
/***************************************** * WiringPi launcher to set PWM * * based on James Ward PWM settings * * to work with SSR and Espresso machine * *****************************************/ int main (int argc, char *argv[]) { int drive = 0; int reto = 0; if(argc != 2) { printf("Error: invalid arguments. Usage: sudo pwmlauncher <aaa> where aaa is the drive percent\n"); exit(0); } //printf ("Welcome into PWM drive launcher\n") ; //retrieve argument drive = atoi(argv[1]); if(drive > 100) drive = 100; printf("drive=%d%\n",drive); //init wiringpi if (wiringPiSetupGpio() == -1){ printf("Error inint wiringpi lib\n"); exit (1) ; } //PWM mode pinMode(18,PWM_OUTPUT); //PWM "predictive mode" pwmSetMode(PWM_MODE_MS); //set clock at 2Hz (clock divider / range) pwmSetClock(PWCLKDIVIDER); pwmSetRange (PWRANGE) ; //setting drive according to the RANGE pwmWrite (18, (drive * (PWRANGE / 100))); }
int main(int argc, const char *argv[]) { unsigned int timeBase = 3000000; char buffer[10]; char prevLine[10]; if (geteuid() != 0) { // chown root <file> // sudo chmod u+s <file> char *exec = rindex(argv[0], '/'); exec++; fprintf(stderr, "You must be root to run \"%s\". Program should be suid root. This is an error.\n", exec) ; return 1; } atexit(destroy); signal(SIGHUP, terminated); signal(SIGINT, terminated); signal(SIGKILL, terminated); signal(SIGPIPE, terminated); signal(SIGALRM, terminated); signal(SIGTERM, terminated); wiringPiSetupGpio(); pinMode(18, PWM_OUTPUT); pwmSetMode(PWM_MODE_MS); pwmWrite(18, 0); char tone[3] = "--"; int octave; unsigned int duration; char *line = fgets(buffer, 10, stdin); assert(line != NULL); int parsed = sscanf(line, "%c%c%i %i", &tone[0], &tone[1], &octave, &duration); while (parsed == 4) { printf("%s%i, 1/%i\n", tone, octave, duration); fflush(stdout); int divisor = 1 << (8 - octave - 2); int toneIndex = 0; for (toneIndex = 0; toneIndex < 13; toneIndex++) { if (0 == strncmp(tone, TONE_NAME[toneIndex], 2)) { break; } } assert(toneIndex < 13); if (toneIndex < 12) { float frequency = TONE_HZ[toneIndex] / divisor; unsigned int range = (unsigned int)(600000.0f / frequency); pwmSetRange(range); pwmWrite(18, range >> 1); usleep(timeBase / duration); pwmWrite(18, 0); usleep(20000); } else {
// IZQUIERDO > 73 hacia adelante // DERECHO < 73 hacia adelante int main (void) { // para usar el modo PWM por hardware iniciamos el WiringPi con numeracion GPIO wiringPiSetupGpio () ; //iniciamos los sensores como INPUT pinMode (SENSOR_DER,INPUT); pinMode (SENSOR_IZQ,INPUT); //iniciamos los servos como PWM pinMode (SERVO_DER,2); pwmSetMode(0); pwmSetClock(400); pwmSetRange(1024); pinMode(SERVO_IZQ,2); pwmSetMode(0); pwmSetClock(400); pwmSetRange(1024); //Bucle while(1) { // el circuito esta hecho para que de Alta en blanco // por lo que si el sensor derecho pasa a baja (Negro) Gira a la derecha if(!digitalRead(SENSOR_DER)){ //printf("SI DERECHA\n"); pwmWrite(SERVO_DER,77); pwmWrite(SERVO_IZQ,80); delay(1); } //si el sensor izquierdo pasa a baja (Negro) Gira a la izquierda else if(!digitalRead(SENSOR_IZQ)) { //printf("SI IZQUIERDA\n"); pwmWrite(SERVO_IZQ,71); pwmWrite(SERVO_DER,40); delay(1); } // si ambos estan en alta, continua recto. else{ //printf("NO\n"); pwmWrite(SERVO_DER,66); pwmWrite(SERVO_IZQ,80); delay(1); } } return 0; }
//========================================================================== // Class: PWMOutput // Function: SetRange // // Description: Sets the PWM range (resolution). Default is 1024. // // Input Arguments: // range = unsigned int // // Output Arguments: // None // // Return Value: // None // //========================================================================== void PWMOutput::SetRange(unsigned int range) { assert(range <= maxRange); pwmSetRange(range); this->range = range; SetDutyCycle(duty); }
/* Set up PWM output */ void setupPWM() { if (wiringPiSetup () == -1) exit (1); pinMode(1, PWM_OUTPUT); pwmSetMode(PWM_MODE_MS);// Use Mark-Space mode for constant frequency output pwmSetClock(384); // This gives a clock of 50kHz pwmSetRange(1000); // This divides the clock to 50Hz (RC compatible) }
/* pwmSetRange * * Parameters: * - range: unsigned int * Return Type: void */ mrb_value mrb_Pi_pwmSetRange(mrb_state* mrb, mrb_value self) { mrb_int native_range; /* Fetch the args */ mrb_get_args(mrb, "i", &native_range); /* Invocation */ pwmSetRange(native_range); return mrb_nil_value(); }
HardwareMotor::HardwareMotor(int setPin1, int setPin2, int setPinPWM) : pin1(setPin1), pin2(setPin2), pinPWM(setPinPWM) { // H-bridge control pins pinMode(pin1, OUTPUT); pinMode(pin2, OUTPUT); // PWM setup pinMode (pinPWM, PWM_OUTPUT); pwmSetMode(PWM_MODE_MS); pwmSetClock(16); pwmSetRange(100); }
int main (void) { if (wiringPiSetupGpio() == -1) exit (1) ; printf ("Setting pin 12 to %dHz at %d%% duty cycle \n",FREQ, DUTY) ; pwmSetMode(PWM_MODE_MS); //set pwm mode to mark-space. PWM_MODE_BAL does really weird things pinMode(18,PWM_OUTPUT); pwmSetClock(19200000/(2*FREQ)); pwmSetRange (100) ; pwmWrite (18, DUTY); // 18 corresponds to physical pin 12 delay (1000); }
int main() { if (wiringPiSetupGpio() == -1) { std::cout << "cannot setup gpio." << std::endl; return 1; } pinMode(18, PWM_OUTPUT); pwmSetMode(PWM_MODE_MS); pwmSetClock(64); pwmSetRange(100); pwmWrite(18, 50); return 0; }
void setup() { wiringPiSetupGpio(); pinMode(TRIG, OUTPUT); pinMode(ECHO, INPUT); //TRIG pin must start LOW digitalWrite(TRIG, LOW); delay(30); //seta motor pwmSetMode(PWM_MODE_MS); pwmSetRange(1024); pwmSetClock(375); pwmWrite(MOTOR0,975); pwmWrite(MOTOR1,975); }
int main(int argc, char ** argv) { wiringPiSetup(); pinMode(1, PWM_OUTPUT); pwmSetMode(PWM_MODE_MS); pwmSetClock(400); pwmSetRange(1000); //pwmWrite(1, 30); delay(1000); //pwmWrite(1, 120); delay(1000); pwmWrite(1, 60); delay(1000); pwmWrite(1, 90); delay(1000); pwmWrite(1, 75); delay(1000); return 0; }
static foreign_t pl_pwmSetRange (term_t range_){ unsigned int range; if (!PL_get_integer(range_,&range)) { PL_warning("Argument `range` not number!\n"); PL_fail; } if (range<0) { PL_warning("Argument 'range' not unsigned one!\n"); PL_fail; } pwmSetRange(range); PL_succeed; }
/*左モータ情報の初期化*/ int Motor_Init_Left(motor_state_info* motor_state_left ) { motor_state_left -> id = MOTOR_LEFT; motor_state_left -> pwm_advance_pin = LEFTMOTORADVANCEPIN; motor_state_left -> pwm_reverse_pin = LEFTMOTORREVERSEPIN; printf("Motor_Init_Left pwm_advance_pin = %u\r\n",motor_state_left -> pwm_advance_pin); printf("Motor_Init_Left pwm_reverse_pin = %u\r\n",motor_state_left -> pwm_reverse_pin); pinMode(RIGHTMOTORADVANCEPIN, PWM_OUTPUT); pwmSetMode (PWM_MODE_MS );// not Balanced mode but Mark:Space mode pwmSetClock(CLOCK); pwmSetRange(RANGE); // setting SW PWM softPwmCreate(LEFTMOTORADVANCEPIN,0,PWM_RANGE); //advance softPwmCreate(LEFTMOTORREVERSEPIN,0,PWM_RANGE);//reverse //softPwmCreate(LEFTMOTORREVERSEPIN,0,RANGE);//reverse return 0; }
void initRudder(void) { // Initialize ad-converter int adcHandle; pinMode(6, INPUT); pullUpDnControl(6, PUD_OFF); adcHandle = wiringPiSPISetup(0, 1000000); // Channel 0, speed 1 MHz, get filedescriptor printf("Filedescriptor: %d\n", adcHandle); char mode = SPI_MODE_1; int ioc = ioctl(adcHandle, SPI_IOC_WR_MODE, &mode); // Set mode = 1 printf("Mode set return should be > -1: %d\n", ioc); ioc = ioctl(adcHandle, SPI_IOC_RD_MODE, &mode); // Check mode = 1 printf("Mode get return should be = 1: %d\n", mode); // Write contents of control registers unsigned char ctrl[5]; ctrl[0] = 0x43; // WREG cmd, write 4 register starting at 0x00 ctrl[1] = 0x80; ctrl[2] = 0xC4; ctrl[3] = 0xC0; ctrl[4] = 0x00; ioc = wiringPiSPIDataRW(0, ctrl, 5); printf("Mode set return should be > -1: %d\n", ioc); // Send start/sync command 0x08 unsigned char cmd[2]; cmd[0] = 0x08; cmd[1] = 0x00; ioc = wiringPiSPIDataRW(0, cmd, 1); printf("Start/sync command, return should be > -1: %d\n", ioc); // Initialize PWM outputs pinMode(25, OUTPUT); // EA pinMode(1, PWM_OUTPUT); // PA pinMode(24, PWM_OUTPUT); // PB pwmSetMode( PWM_MODE_MS); pwmSetRange(1024); pwmSetClock(32); }
static void doPwmRange (int argc, char *argv []) { unsigned int range ; if (argc != 3) { fprintf (stderr, "Usage: %s pwmr <range>\n", argv [0]) ; exit (1) ; } range = (unsigned int)strtoul (argv [2], NULL, 10) ; if (range == 0) { fprintf (stderr, "%s: range must be > 0\n", argv [0]) ; exit (1) ; } pwmSetRange (range) ; }
void Gpio::usePibrellaPwm() { if (mHwPwmInit) return; // Setup hardware PWM for the buzzer (we need to control the frequency). // Note that the buzzer is on wiring pi pin 1 which is the only pin that // can be used for hardware PWM. pinMode(HW_PWM_PIN, PWM_OUTPUT); // Switch hardware PWM to mark-space mode pwmSetMode(PWM_MODE_MS); // Need a square wave so set range to 100 so value 50 = 50% duty cycle pwmSetRange(100); // Turn PWM off initially pwmWrite(HW_PWM_PIN, 0); mHwPwmInit = true; }
int main(int argc, char* argv[]) { printf("Press Maru-Button to Exit Process.\n"); int iRet = 0; CJoystickDrv* pJoystick = NULL; try { // ready GPIO if( wiringPiSetupGpio() == -1 ){ printf("failed to wiringPiSetupGpio()\n"); return 1; } // ready PWM pinMode(GPIO_NO, PWM_OUTPUT); pwmSetMode(PWM_MODE_MS); pwmSetClock(DEF_PWM_CLOCK); pwmSetRange(DEF_PWM_RANGE); // ready Joystick pJoystick = CJoystickDrv::createInstance(); if(!pJoystick){ throw 0; } if(pJoystick->connectJoystick()!=0){ printf("failed to connectJoystick()\n"); throw 0; } const int axis_min = DUALSHOCK_ANALOG_VAL_MIN; const int axis_max = DUALSHOCK_ANALOG_VAL_MAX; const int axis_mid = DUALSHOCK_ANALOG_VAL_MID; const int servo_min = SERVO_MIN; const int servo_max = SERVO_MAX; const int servo_mid = SERVO_MID; pwmWrite(GPIO_NO, servo_mid); printf("begin loop \n"); int pre_val = servo_mid; while(1){ // Joystickの状態を更新 if( pJoystick->readJoystick()!=0 ){ printf("faile to readJoystick()\n"); throw 0; } int roll = pJoystick->getAxisState(0); int val = servo_mid; if(roll==axis_mid){ // 中間値 val = servo_mid; }else if(roll > axis_mid){ // 右 double rate = fabs( (double)roll / (double)(axis_max) ); int delta = (int)( (double)( servo_mid - servo_min) * rate ); val = servo_mid - delta; if(val < servo_min){ val = servo_min; } }else if(roll < axis_mid){ // 左 double rate = fabs( (double)roll / (double)(axis_min) ); int delta = (int)( (double)( servo_max - servo_mid ) * rate ); val = servo_mid + delta; if(val > servo_max){ val = servo_max; } } if( pre_val != val ){ pwmWrite(GPIO_NO, val); usleep(DELAY_USEC); pre_val = val; } //Maru if(pJoystick->getButtonState(JOY_MARU) == BUTTON_ON){ printf("pushed Maru-Button\n"); break; } sleep(0); } printf("end loop\n"); pwmWrite(GPIO_NO, servo_mid); if(pJoystick){ delete pJoystick; pJoystick = NULL; } } catch(...) { printf("catch!! \n"); iRet = -1; if(pJoystick){ delete pJoystick; pJoystick = NULL; } } return iRet; }
int main(int argc, char* argv[]) { #if ( USE_TALK > 0 ) system("/home/pi/aquestalkpi/AquesTalkPi -g 60 \"まっすんどろいど を きどうします\" | aplay"); #endif #if ( USE_TALK_TEST > 0 ) int i=0; for(i=0;i<TALK_REASON_NUM;i++){ talkReason(i); } for(i=0;i<TALK_WELCOME_NUM;i++){ talkWelcome(i); } #endif printf("Press Esc-Key to Exit Process.\n"); int iRet = -1; try { // ready GPIO if( wiringPiSetupGpio() == -1 ){ printf("failed to wiringPiSetupGpio()\n"); throw 0; } // ready PWM pinMode(GPIO_PITCH, PWM_OUTPUT); pinMode(GPIO_YAW, PWM_OUTPUT); pwmSetMode(PWM_MODE_MS); pwmSetClock(400); pwmSetRange(1024); pinMode(GPIO_EXIT, INPUT); pinMode(GPIO_HALT, INPUT); //pinMode(GPIO_MONOEYE, OUTPUT); //digitalWrite(GPIO_MONOEYE,HIGH); // servoMotor GWS park hpx min25 mid74 max123 const int servo_mid = 76; const int servo_min = 36; //servo_mid - 30; const int servo_max = 122; //servo_mid + 30; const double servo_min_deg = 0.0; const double servo_max_deg = 180.0; const double ratio_deg = ( servo_max_deg - servo_min_deg ) / ( servo_max - servo_min ); const int pitch_limit_max = servo_max - 22; const int pitch_limit_min = servo_min + 34; RASPIVID_CONFIG * config = new RASPIVID_CONFIG(); if(!config){ printf("failed to create RASPIDVID_CONFIG.\n"); return -1; } config->width=static_cast<int>(WIN_WIDTH); config->height=static_cast<int>(WIN_HEIGHT); config->bitrate=0; // zero: leave as default config->framerate=0; config->monochrome=0; config->rotation=90; // using https://github.com/IsaoNakamura/robidouille.git #if ( USE_WIN > 0 ) cvNamedWindow( DISP_WIN , CV_WINDOW_AUTOSIZE ); #endif RaspiCamCvCapture* capture = NULL; if (argc > 1){ //capture = raspiCamCvCreateCameraCapture2( argv[1], config ); throw 0; }else{ capture = raspiCamCvCreateCameraCapture2( 0, config ); if(config){ delete config; config = NULL; } if(!capture){ printf("failed to create capture\n"); throw 0; } // キャプチャサイズを設定する. double w = WIN_WIDTH; double h = WIN_HEIGHT; raspiCamCvSetCaptureProperty (capture, RPI_CAP_PROP_FRAME_WIDTH, w); raspiCamCvSetCaptureProperty (capture, RPI_CAP_PROP_FRAME_HEIGHT, h); } // 正面顔検出器の読み込み CvHaarClassifierCascade* cvHCC = (CvHaarClassifierCascade*)cvLoad(CASCADE, NULL,NULL,NULL); // 検出に必要なメモリストレージを用意する CvMemStorage* cvMStr = cvCreateMemStorage(0); // サーボ角度を中間に設定 pwmWrite(GPIO_YAW, servo_mid); pwmWrite(GPIO_PITCH, servo_mid); // スクリーン座標からカメラのピッチ角とヨー角を算出するオブジェクトを初期化 DF::CamAngleConverter camAngCvt( static_cast<int>(WIN_WIDTH), static_cast<int>(WIN_HEIGHT), ANGLE_DIAGONAL ); if (!camAngCvt.Initialized()) { printf("failed to initialize CamAngleConverter.\n"); throw 0; } struct timeval stNow; // 現時刻取得用 struct timeval stLen; // 任意時間 struct timeval stEnd; // 現時刻から任意時間経過した時刻 timerclear(&stNow); timerclear(&stLen); timerclear(&stEnd); unsigned int msec = HOMING_DELAY_MSEC; gettimeofday(&stNow, NULL); stLen.tv_sec = msec / 1000; stLen.tv_usec = msec % 1000; timeradd(&stNow, &stLen, &stEnd); srand(stNow.tv_usec); // 前値保存用のサーボ角度 int _servo_yaw = servo_mid; int _servo_pitch = servo_mid; // スクリーン中心らへんの範囲 double center_area_x = 60.0; double center_area_y = 60.0; int homing_state = HOMING_NONE; int over_cnt = 0; int nonface_cnt = 0; int silent_cnt = 0; // メインループ while(1){ int wrk_homing_state = HOMING_NONE; IplImage* frame = raspiCamCvQueryFrame(capture); if(!frame){ printf("failed to query frame.\n"); break; } // 画像中から検出対象の情報を取得する CvSeq* face = cvHaarDetectObjects( frame , cvHCC , cvMStr , 1.2 , 2 , CV_HAAR_DO_CANNY_PRUNING , minsiz , minsiz ); if(!face){ printf("failed to detect objects.\n"); break; } int i=0; //for(i = 0; i < face->total; i++) { if( face->total > 0 ){ nonface_cnt = 0; i=0; // 最初のひとつの顔だけ追尾ターゲットにする // 検出情報から顔の位置情報を取得 CvRect* faceRect = (CvRect*)cvGetSeqElem(face, i); if(!faceRect){ printf("failed to get Face-Rect.\n"); break; } center_area_x = faceRect->width / 2.0 * CENTER_AREA_RATIO; center_area_y = faceRect->height / 2.0 * CENTER_AREA_RATIO; #if ( USE_WIN > 0 ) // スクリーン中心らへん矩形描画を行う cvRectangle( frame , cvPoint( (WIN_WIDTH_HALF - center_area_x), (WIN_HEIGHT_HALF - center_area_x) ) , cvPoint( (WIN_WIDTH_HALF + center_area_y), (WIN_HEIGHT_HALF +center_area_y) ) , CV_RGB(0, 255 ,0) , 2 , CV_AA , 0 ); // 取得した顔の位置情報に基づき、矩形描画を行う cvRectangle( frame , cvPoint(faceRect->x, faceRect->y) , cvPoint(faceRect->x + faceRect->width, faceRect->y + faceRect->height) , CV_RGB(255, 0 ,0) , 2 , CV_AA , 0 ); #endif // 顔のスクリーン座標を算出 double face_x = faceRect->x + (faceRect->width / 2.0); double face_y = faceRect->y + (faceRect->height / 2.0); if( face_x >= (WIN_WIDTH_HALF - center_area_x) && face_x <= (WIN_WIDTH_HALF + center_area_x) && face_y >= (WIN_HEIGHT_HALF - center_area_y) && face_y <= (WIN_HEIGHT_HALF + center_area_y) ){ wrk_homing_state = HOMING_CENTER; }else{ // 顔がスクリーン中心らへんになければ処理を行う // 現在時刻を取得 gettimeofday(&stNow, NULL); if( timercmp(&stNow, &stEnd, >) ){ // 任意時間経てば処理を行う // スクリーン座標からカメラのピッチ・ヨー角を算出 double deg_yaw = 0.0; double deg_pitch = 0.0; if( camAngCvt.ScreenToCameraAngle(deg_yaw, deg_pitch, face_x, face_y) != 0 ){ continue; } printf("face(%f,%f) deg_yaw=%f deg_pitch=%f servo(%d,%d)\n",face_x,face_y,deg_yaw,deg_pitch,_servo_yaw,_servo_pitch); // サーボ値を入れる変数 初期値は前回の結果 int servo_yaw = _servo_yaw; int servo_pitch = _servo_pitch; // ヨー角用サーボ制御 //servo_yaw = servo_mid - static_cast<int>(deg_yaw / ratio_deg); // カメラ固定だとこれでよい servo_yaw = _servo_yaw - static_cast<int>(deg_yaw / ratio_deg); if(servo_yaw > servo_max){ over_cnt++; printf("yaw is over max. cnt=%d ######## \n", over_cnt); }else if(servo_yaw < servo_min){ over_cnt++; printf("yaw is under min. cnt=%d ######## \n",over_cnt); servo_yaw = servo_min; } //printf("face_x=%f deg_yaw=%f servo_yaw=%d \n",face_x,deg_yaw,servo_yaw); // ピッチ角用サーボ制御 //servo_pitch = servo_mid - static_cast<int>(deg_pitch / ratio_deg); // カメラ固定だとこれでよい servo_pitch = _servo_pitch - static_cast<int>(deg_pitch / ratio_deg); if(servo_pitch > pitch_limit_max){ over_cnt++; printf("pitch is over max ######## \n"); servo_pitch = pitch_limit_max; }else if(servo_pitch < pitch_limit_min){ over_cnt++; printf("pitch is under min ######## \n"); servo_pitch = pitch_limit_min; } //printf("pwmWrite(%d,%d,%f)\n",servo_yaw,servo_pitch,ratio_deg); bool isPwmWrite = false; // SERVO_OVER_MAXフレーム分の間、サーボ角度が最大が続くのであれば、サーボ角度を中間にもどす。 if( over_cnt > SERVO_OVER_MAX){ servo_yaw=servo_mid; servo_pitch=servo_mid; over_cnt = 0; } // 前回と同じサーボ値ならスキップ if(servo_yaw!=_servo_yaw){ // サーボの角度設定 printf("pwmWrite(GPIO_YAW, %d)\n",servo_yaw); pwmWrite(GPIO_YAW, servo_yaw); isPwmWrite = true; // 前値保存 _servo_yaw = servo_yaw; } if(servo_pitch!=_servo_pitch){ // サーボの角度設定 printf("pwmWrite(GPIO_PITCH, %d)\n",servo_pitch); pwmWrite(GPIO_PITCH, servo_pitch); isPwmWrite = true; // 前値保存 _servo_pitch = servo_pitch; } if( isPwmWrite ){ // サーボの値を設定したら現時刻から任意時間プラスして、サーボの角度設定しない終了時刻を更新 timerclear(&stEnd); timeradd(&stNow, &stLen, &stEnd); wrk_homing_state = HOMING_HOMING; }else{ wrk_homing_state = HOMING_KEEP; } }else{ // if( timercmp(&stNow, &stEnd, >) ) wrk_homing_state = HOMING_DELAY; } } // if(face_x ... ){}else }else{ // if( face->total > 0 ) wrk_homing_state = HOMING_NONE; // NONFACE_CNT_MAXフレーム分の間、顔検出されなければ、サーボ角度を中間にもどす。 nonface_cnt++; silent_cnt++; #if ( USE_TALK > 0 ) if( silent_cnt > SILENT_CNT ){ silent_cnt = 0; //digitalWrite(GPIO_MONOEYE,HIGH); int talkType = rand() % TALK_REASON_NUM; talkReason(talkType); //digitalWrite(GPIO_MONOEYE,LOW); } #endif if( nonface_cnt > NONFACE_CNT_MAX ){ nonface_cnt = 0; int servo_yaw = servo_mid; int servo_pitch = servo_mid; // サーボの角度設定 printf("pwmWrite(GPIO_YAW, %d) for non-face. \n",servo_yaw); pwmWrite(GPIO_YAW, servo_yaw); //isPwmWrite = true; // 前値保存 _servo_yaw = servo_yaw; // サーボの角度設定 printf("pwmWrite(GPIO_PITCH, %d) for non-face. \n",servo_pitch); pwmWrite(GPIO_PITCH, servo_pitch); //isPwmWrite = true; // 前値保存 _servo_pitch = servo_pitch; } } // ホーミング状態を更新 if(homing_state != wrk_homing_state){ int talkType = 0; switch( wrk_homing_state ) { case HOMING_NONE: printf("[STATE] no detected face.\n"); /* #if ( USE_TALK > 0 ) if( homing_state != HOMING_DELAY ){ digitalWrite(GPIO_MONOEYE,HIGH); talkType = rand() % TALK_REASON_NUM; talkReason(talkType); digitalWrite(GPIO_MONOEYE,LOW); } #endif */ break; case HOMING_HOMING: printf("[STATE] homing.\n"); #if ( USE_TALK > 0 ) //digitalWrite(GPIO_MONOEYE,HIGH); talkType = rand() % TALK_WELCOME_NUM; talkWelcome(talkType); //digitalWrite(GPIO_MONOEYE,LOW); silent_cnt = 0; #endif break; case HOMING_DELAY: printf("[STATE] delay.\n"); break; case HOMING_CENTER: printf("[STATE] face is center.\n"); #if ( USE_TALK > 0 ) ////digitalWrite(GPIO_MONOEYE,HIGH); //talkType = rand() % TALK_WELCOME_NUM; //talkWelcome(talkType); ////digitalWrite(GPIO_MONOEYE,LOW); //silent_cnt = 0; #endif break; case HOMING_KEEP: printf("[STATE] keep.\n"); break; default: break; } homing_state = wrk_homing_state; } #if ( USE_WIN > 0 ) // 画面表示更新 cvShowImage( DISP_WIN, frame); #endif // 負荷分散のためDelay char c = cvWaitKey(DELAY_SEC); if( c==27 ){ // ESC-Key printf("exit program.\n"); #if ( USE_TALK > 0 ) system("/home/pi/aquestalkpi/AquesTalkPi -g 60 \"ぷろぐらむを しゅうりょう します\" | aplay"); #endif break; } if( digitalRead(GPIO_EXIT) == LOW ){ printf("exit program.\n"); #if ( USE_TALK > 0 ) system("/home/pi/aquestalkpi/AquesTalkPi -g 60 \"ぷろぐらむを しゅうりょう します\" | aplay"); #endif break; } if( digitalRead(GPIO_HALT) == LOW ){ #if ( USE_TALK > 0 ) system("/home/pi/aquestalkpi/AquesTalkPi -g 60 \"しすてむを しゃっとだうん します\" | aplay"); #endif printf("shutdown system.\n"); system("sudo halt"); break; } } // while(1)
int main(void) { /* if(wiringPiSetupGpio()==-1) return 1; int gpio1 = 27; softPwmCreate(gpio1, 0, 50); usleep(100000); while (1) { softPwmWrite(gpio1, 10); usleep(100000); } return 0; */ //if(wiringPiSetupGpio()==-1) //return 1; const int desiredDistFromWall = 110 ; wiringPiSetup(); pinMode(gpio_steering,PWM_OUTPUT); pinMode(gpio_lidarPan,PWM_OUTPUT); pwmSetMode(PWM_MODE_MS); pwmSetClock(375); pwmSetRange (1024); softPwmCreate(gpio1, 0, 200); softPwmCreate(gpio2, 0, 200); //softPwmCreate(gpio_pixyPan , 0, 200); //softPwmCreate(gpio_pixyTilt, 0, 200); softPwmCreate(gpio_lidarTilt,0, 200); //initializeServos(gpio_steering,gpio_pixyPan,gpio_pixyTilt,gpio_lidarPan,gpio_lidarTilt); int lidFd = wiringPiI2CSetup(0x62); int fd = wiringPiI2CSetup(0x30); int newFd = changeEncoderAddress(fd, 0x10); //printf("fd = %d\r\n", newFd); double result = readEncoder(newFd); int fd2 = wiringPiI2CSetup(0x30); int newFd2 = changeEncoderAddress(fd2, 0x11); //printf("fd2 = %d\r\n", newFd2); double result2 = readEncoder(newFd2); double dt = 100000; double integral = 0; double previousError = 0; double Kp = 0.01; double Ki = 0.0008; double Kd = 1; double error = 0; double derivative = 0; double output = 0; int lidVal = 0; int lidarPanServoInd [3] = {20 , 50, 70}; int pixyPanServoInd [3] = {0 , 5, 10}; // setServo(gpio_pixyTilt,22); // usleep(30000); // setServo(gpio_pixyTilt,0); setServo(gpio_lidarTilt,20); usleep(30000); setServo(gpio_lidarTilt,0); setHardServo(gpio_lidarPan,70); double avg = desiredDistFromWall; int errFromWall=0; for (int i = 0; i < 200; i++) { lidVal = readLidar (lidFd); if (lidVal >0 ) { // steering based on lidVal avg = (lidVal+avg*3)/(3.0+1) ; } errFromWall = avg-desiredDistFromWall; if (errFromWall>5) setHardServo(gpio_steering,60); else if (errFromWall<-5) setHardServo(gpio_steering,40); else setHardServo(gpio_steering,50); printf ("\n lidVal: %d",lidVal) ; printf ("\n Avg distance: %f",avg) ; //setServo(gpio_steering,i); result = readEncoder(newFd); result2 = readEncoder(newFd2); error = result + result2 ; integral += error ; derivative = (error - previousError)/dt ; output = Kp*error + Ki*integral + Kd * derivative; printf ("\n LEFT ENCODER: %f -- RIGHT encoder: %f -- Error: %f \n", result,result2,error); moveForward (17 , output) ; usleep(dt); } setHardServo(gpio_steering,0); setHardServo(gpio_lidarPan,0); return 0; }