/*****************************************
* 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)));
}
/**
* Plays the specified MIDI note number where 60 = Middle C, 69 = A (440 Hz).
* Note 0 is a rest (silence) and note 1 is a buzz (low freq).
*/
void Gpio::startBuzzer(int note)
{
if (!mHwPwmInit)
usePibrellaPwm();
if (note == 0){
pwmWrite(HW_PWM_PIN, 0);
mBuzzerClock = 0;
return;
}
// Convert MIDI note number to frequency
double freq;
if (note == 1)
freq = 20;
else
freq = 440.0 * pow(2.0, ((note - 69) / 12.0));
// Fiddle the clock value so that it sounds good from 1 octave below
// middle C to one octave above, i.e. 3 full octaves.
int clock = (int)(60000.0 / freq);
if (mBuzzerClock != clock){
pwmSetClock(clock);
pwmWrite(HW_PWM_PIN, 50);
mBuzzerClock = clock;
}
}
// 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;
}
/* 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)
}
//==========================================================================
// Class: PWMOutput
// Function: SetFrequency
//
// Description: Sets the PWM frequency. Note that the frequency
// may be rounded and thus not exactly as specified.
// The achievable frequencies are dependent on the
// range - higher ranges allow lower frequencies.
// If the frequency cannot be achieved with the current
// range, this function returns false.
//
// This method seems to work pretty well, but I have no proof
// suggesting that it will always generate the best pair or
// range-clock divisor values. That criteria is somewhat
// dependent on your valuationg of resolution vs. frequency
// accuracy anyway.
//
// Input Arguments:
// frequency = double [Hz]
// minResolution = unsigned int
//
// Output Arguments:
// None
//
// Return Value:
// bool, true if successfully set, false otherwise (frequency out of
// bounds or wrong PWM mode)
//
//==========================================================================
bool PWMOutput::SetFrequency(double frequency, unsigned int minResolution)
{
unsigned int newRange, divisor;
if (mode == ModeMarkSpace)
{
const unsigned int rangeDivisorProduct = floor(pwmClockFrequency / frequency + 0.5);
divisor = 1;
// Make sure the frequency is within the range we can attempt
if (rangeDivisorProduct / minResolution < minClockDivisor ||// Frequency too high
rangeDivisorProduct / maxClockDivisor > maxRange)// Frequency too low
return false;
unsigned int i(1);
while (divisor < 2 || newRange > maxRange ||
divisor > maxClockDivisor || newRange < minResolution)
{
// If we've tried all combinations, we're done
if (i >= 2 * rangeDivisorProduct)
return false;
if (i % 2 == 0)
divisor = GetMinimumAcceptableFactor(rangeDivisorProduct + floor(i / 2.0));
else
divisor = GetMinimumAcceptableFactor(rangeDivisorProduct - floor(i / 2.0));
i++;
newRange = floor(rangeDivisorProduct / divisor + 0.5);
}
}
else
{
// See page 139 of the Broadcom ARM documentation. The default PWM mode
// (balanced) DOES in fact change the apparent frequency as a function of
// duty cycle. Their goal is to achieve as even a distribution of on/off
// pulses as possible within any arbitrary block of time (i.e. when the
// arbitrary time slice is not an integer multiple of the PWM period).
// For this reason, we won't attempt to implement SetFrequency for balanced
// PWM mode (although it does have an affect on the PWM output).
// http://www.element14.com/community/servlet/JiveServlet/downloadBody/43016-102-1-231518/Broadcom.Datasheet.pdf
//assert(false);
return false;
}
assert(divisor >= minClockDivisor &&
divisor <= maxClockDivisor &&
newRange >= minResolution);
pwmSetClock(divisor);
SetRange(newRange);
frequency = pwmClockFrequency / (double)divisor / (double)newRange;
return true;
}
/* pwmSetClock
*
* Parameters:
* - divisor: int
* Return Type: void
*/
mrb_value
mrb_Pi_pwmSetClock(mrb_state* mrb, mrb_value self) {
mrb_int native_divisor;
/* Fetch the args */
mrb_get_args(mrb, "i", &native_divisor);
/* Invocation */
pwmSetClock(native_divisor);
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);
}
static foreign_t
pl_pwmSetClock (term_t divisor_){
int divisor;
if (!PL_get_integer(divisor_,&divisor)) {
PL_warning("Argument `divisor` not number!\n");
PL_fail;
}
pwmSetClock(divisor);
PL_succeed;
}
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);
}
void init_PWM(void)
{
int i;
i = wiringPiSetup();
if(i < 0)
{
printf("Necazuri la initializare PWM\n");
exit(1);
}
pinMode(1, PWM_OUTPUT);
pwmSetMode(PWM_MODE_MS);
pwmSetClock(PWM_DIVISOR);
pwmWrite(1, 0); // do nothing for the moment
}
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;
}
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;
}
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);
}
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 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 doPwmClock (int argc, char *argv [])
{
unsigned int clock ;
if (argc != 3)
{
fprintf (stderr, "Usage: %s pwmc <clock>\n", argv [0]) ;
exit (1) ;
}
clock = (unsigned int)strtoul (argv [2], NULL, 10) ;
if ((clock < 1) || (clock > 4095))
{
fprintf (stderr, "%s: clock must be between 0 and 4096\n", argv [0]) ;
exit (1) ;
}
pwmSetClock (clock) ;
}
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(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;
}
//it is fast off
void output_38k_off()
{
pwmSetClock(0);
}
//it is slow begin
void output_38k()
{
pwmSetClock(252);
}
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;
}
