Esempio n. 1
0
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);

}
Esempio n. 2
0
/*****************************************
 * 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))); 
}
Esempio n. 3
0
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 {
Esempio n. 4
0
// 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;
}
Esempio n. 5
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);
}
Esempio n. 6
0
/* 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)
}
Esempio n. 7
0
/* 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();
}
Esempio n. 8
0
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);
}
Esempio n. 9
0
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);
}
Esempio n. 10
0
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);
}
Esempio n. 12
0
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;
}
Esempio n. 13
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;
}
Esempio n. 14
0
/*左モータ情報の初期化*/
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;
}
Esempio n. 15
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);
}
Esempio n. 16
0
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) ;
}
Esempio n. 17
0
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;
}
Esempio n. 18
0
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;
}
Esempio n. 19
0
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)
Esempio n. 20
0
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;
}