int main(int argc, char *argv[])
{
float dis;
// char buf[BUFSIZE]={0xff,0x00,0x00,0x00,0xff};
int SR;
int SL;
/*RPI*/
wiringPiSetup();
/*WiringPi GPIO*/
pinMode (1, OUTPUT); //IN1
pinMode (4, OUTPUT); //IN2
pinMode (5, OUTPUT); //IN3
pinMode (6, OUTPUT); //IN4
// pinMode (27, OUTPUT); //
softPwmCreate(1,1,500);
softPwmCreate(4,1,500);
softPwmCreate(5,1,500);
softPwmCreate(6,1,500);
// softPwmCreate(27,1,50);
//softPwmWrite(27,1);
while(1)
{
//有信号为LOW 没有信号为HIGH
SR = digitalRead(RIGHT);//
SL = digitalRead(LEFT);//
if (SL == LOW&&SR==LOW){
printf("BACK"); //前面有物体时小车后退？？？ms 再转弯
back();
delay(300);
//后退500ms
left();//左转400ms
delay(601);
}
else if (SL == HIGH&&SR == LOW){
printf("RIGHT");
left();
}
else if (SR == HIGH&&SL == LOW) {//
printf("LEFT");
right ();
}
else {// 前面没有物体 前进
printf("GO");
run();
}
}
return 0;
}
int main(void) {
wiringPiSetup();
if (0 != softPwmCreate (AIA_PIN, 0, 100))
printf("Falied to create pwm pin AIA");
if (0 != softPwmCreate (AIB_PIN, 0, 100))
printf("Falied to create pwm pin AIB");
if (0 != softPwmCreate (BIA_PIN, 0, 100))
printf("Falied to create pwm pin BIA");
if (0 != softPwmCreate (BIB_PIN, 0, 100))
printf("Falied to create pwm pin BIB");
int speed = 0;
char dir = 'f';
while (1) {
scanf("%c %d", &dir, &speed);
if (dir == 'f') {
forward(speed);
} else if (dir == 'b') {
backward(speed);
} else if (dir == 'r') {
right(speed);
} else if (dir == 'l') {
left(speed);
}
printf("Dir set %c, Speed set %d\n", dir, speed);
}
return 0;
}
L293d::L293d(motor_s motor) : motor(motor) {
softPwmCreate(motor.pin1, 0, motor.pwmRange);
softPwmCreate(motor.pin2, 0, motor.pwmRange);
direction = true;
running = false;
}
void main() {
char *df = "/dev/input/js0";
struct ps3ctls ps3dat;
wiringPiSetup();
softPwmCreate( 5,0,20); // start-0 10ms
softPwmCreate( 6,0,20); // start-0 10ms
softPwmCreate(26,0,20); // start-0 10ms
softPwmCreate(27,0,20); // start-0 10ms
softPwmCreate(28,0,20); // start-0 10ms
softPwmCreate(29,0,20); // start-0 10ms
softPwmCreate(14,0,20); // start-0 10ms
softPwmCreate(23,0,20); // start-0 10ms
softPwmCreate(24,0,20); // start-0 10ms
softPwmCreate(25,0,20); // start-0 10ms
pinMode(15,OUTPUT);
digitalWrite(15,0);
fds = wiringPiI2CSetup(0x40); // PCA9685
resetPCA9685(fds);
setPCA9685Freq(fds,50);
while(1) {
if(!(ps3c_init(&ps3dat, df))) {
do {
if (ps3c_test(&ps3dat) < 0) break;
} while (!(ps3c_input(&ps3dat)));
ps3c_exit(&ps3dat);
};
};
}
/**
* Function main
* @author Danny
* @date 2017.08.16
* @brief delay 2s，advance 1s，back 1s,turn left 2s,turn right 2s,
* turn left in place 3s, turn right in place 3s,stop 0.5s
* @param[in] void
* @retval void
* @par History
*/
void main()
{
wiringPiSetup();
//Initializethe motor drive IO as the output mode
pinMode(Left_motor_go, OUTPUT);
pinMode(Left_motor_back, OUTPUT);
pinMode(Right_motor_go, OUTPUT);
pinMode(Right_motor_back, OUTPUT);
//int softPwmCreate(int pin,int initialValue,int pwmRange);
softPwmCreate(Left_motor_pwm,0,255);
softPwmCreate(Right_motor_pwm,0,255);
while(1)
{
delay(2000);
run(10);
back(10);
left(20);
right(20);
spin_left(30);
spin_right(30);
brake(5);
}
return;
}
int fnInitializeMotor(int MotorID)
{
/*
* Enable GPIO pins
*/
switch (MotorID)
{
case MOTOR_A:
pinMode(MOTOR_A_PIN1, OUTPUT);
pinMode(MOTOR_A_PIN2, OUTPUT);
pullUpDnControl(MOTOR_A_PIN1, PUD_OFF);
pullUpDnControl(MOTOR_A_PIN2, PUD_OFF);
softPwmCreate(MOTOR_A_PIN1, 0, FULLSPEED);
softPwmCreate(MOTOR_A_PIN2, 0, FULLSPEED);
return 1;
break;
case MOTOR_B:
pinMode(MOTOR_B_PIN1, OUTPUT);
pinMode(MOTOR_B_PIN2, OUTPUT);
pullUpDnControl(MOTOR_B_PIN1, PUD_OFF);
pullUpDnControl(MOTOR_B_PIN2, PUD_OFF);
softPwmCreate(MOTOR_B_PIN1, 0, FULLSPEED);
softPwmCreate(MOTOR_B_PIN2, 0, FULLSPEED);
return 1;
break;
}
return 0;
}
void main() {
char *df = "/dev/input/js0";
struct ps3ctls ps3dat;
wiringPiSetup();
softPwmCreate(5,0,20); // motor-1 10ms
softPwmCreate(6,0,20); // motor-1 10ms
softPwmCreate(26,0,20); // motor-2 10ms
softPwmCreate(27,0,20); // motor-2 10ms
softPwmCreate(28,0,20); // motor-3 10ms
softPwmCreate(29,0,20); // motor-3 10ms
softPwmCreate(24,0,20); // motor-4 10ms
softPwmCreate(25,0,20); // motor-4 10ms
softPwmCreate(14,0,20); // motor-5 10ms // NC
softPwmCreate(23,0,20); // motor-5 10ms // NC
if(!(ps3c_init(&ps3dat, df))) {
do {
if (ps3c_test(&ps3dat) < 0) break;
} while (!(ps3c_input(&ps3dat)));
ps3c_exit(&ps3dat);
}
}
void hardwareSetup() {
wiringPiSetup();
softPwmCreate(pinRed, 0, 100);
softPwmCreate(pinGreen, 0, 100);
softPwmCreate(pinBlue, 0, 100);
softPwmWrite(pinRed, 0);
softPwmWrite(pinGreen, 0);
softPwmWrite(pinBlue, 0);
}
int main (int argc, char *argv[])
{
char buf[4];
if(argc == 2){
InitServer(atoi(argv[1]));
} else {
printf("not enough argument.\nusage: sudo ./a.out <port number>\n\n");
exit(1);
}
if (wiringPiSetup () == -1) exit (1) ;
pinMode (0, OUTPUT);
digitalWrite (0, LOW) ;
int pin = 0; //GPIO 17
if (softPwmCreate(0,0,200) != 0){
fprintf (stdout, "oops: %s\n", strerror (errno)) ;
return 1 ;
}
while(1){
while((n = read(sock,buf,4)) > 0){
Servo(pin, buf);
}
}
return 1;
}
void enablePWM(int pin, int speed)
{
if (0 != softPwmCreate(pin, 0, speed))
{
printf("ERROR: Cannot enable software PWM for pin %d\n", pin);
}
}
// soft PWM
static ERL_NIF_TERM
soft_pwm_create_nif(ErlNifEnv* env, int argc, const ERL_NIF_TERM argv[])
{
int pin, init_value, range, result;
ERL_NIF_TERM atom_fail, err_code;
if (!enif_get_int(env, argv[0], &pin) ||
!enif_get_int(env, argv[1], &init_value) ||
!enif_get_int(env, argv[2], &range))
{
return enif_make_badarg(env);
}
result = softPwmCreate(pin, init_value, range);
if (result)
{
atom_fail = enif_make_atom(env, "failed_to_init_pwm_pin");
err_code = enif_make_int(env, result);
return enif_make_tuple2(env,
atom_error,
enif_make_tuple2(env, atom_fail, err_code));
}
else
{
return atom_ok;
}
}
int main ()
{
int i ;
// setup wirinfPi
if (wiringPiSetup () == -1)
{
fprintf (stdout, "oops: %s\n", strerror (errno)) ;
return 1 ;
}
// initialise the pins
for (i = 0 ; i < NUM_LEDS ; ++i)
{
// initial value = 0
softPwmCreate (ledMap [i], 0, RANGE) ;
softPwmWrite (ledMap [i], values [i]) ;
}
printf ("Wait a few seconds or Ctrl-C to quit\n");
// delay for 5 seconds then exit
for (i = 0 ; i < 5; ++i)
{
delay(i*1000) ;
}
return 0 ;
}
/*右モータ情報の初期化*/
int Motor_Init_Right(motor_state_info* motor_state_right)
{
motor_state_right -> id = MOTOR_RIGHT;
motor_state_right -> pwm_advance_pin = RIGHTMOTORADVANCEPIN;
motor_state_right -> pwm_reverse_pin = RIGHTMOTORREVERSEPIN;
// setting HW PWM for compare;
softPwmCreate(RIGHTMOTORADVANCEPIN,0,PWM_RANGE);//advance
softPwmCreate(RIGHTMOTORREVERSEPIN,0,PWM_RANGE);//reverse
printf("Motor_Init_Right pwm_advance_pin = %u\r\n",motor_state_right -> pwm_advance_pin);
printf("Motor_Init_Right pwm_reverse_pin = %u\r\n",motor_state_right -> pwm_reverse_pin);
return 0;
}
void setupMotor(Motor motor) {
pinMode(motor.pin_1, OUTPUT);
pinMode(motor.pin_2, OUTPUT);
stopMotor(motor);
softPwmCreate(motor.pin_speed, 0, 100);
setSpeedMotor(motor, 0);
}
void init_motors()
{
wiringPiSetup();
// initialize left motor (pin0, 2 & 3)
softPwmCreate(0, INITIAL_VALUE, RANGE);
softPwmCreate(2, INITIAL_VALUE, RANGE);
pinMode(3, OUTPUT);
digitalWrite(3, HIGH);
// initialize right motor (pin4, 5 & 6)
softPwmCreate(5, INITIAL_VALUE, RANGE);
softPwmCreate(4, INITIAL_VALUE, RANGE);
pinMode(6, OUTPUT);
digitalWrite(6, HIGH);
}
void setup() {
if (wiringPiSetup() < 0) {
printf("Error Setup\n");
exit(1);
}
// software pwm
softPwmCreate(0, 0, 100);
softPwmCreate(1, 0, 100);
// direction control
pinMode(2, OUTPUT);
pinMode(3, OUTPUT);
pinMode(4, OUTPUT);
pinMode(5, OUTPUT);
stop();
sleep(1);
}
int main()
{
wiringPiSetup();
// initialize left motor (0:pwm, 2:dir & 3:F)
softPwmCreate(0, INITIAL_VALUE, RANGE);
pinMode(2, OUTPUT);
pinMode(3, OUTPUT);
digitalWrite(2, HIGH);
digitalWrite(3, HIGH);
// initialize right motor (4:pwm, 5:dir & 6:F)
softPwmCreate(4, INITIAL_VALUE, RANGE);
pinMode(5, OUTPUT);
pinMode(6, OUTPUT);
digitalWrite(5, HIGH);
digitalWrite(6, HIGH);
// to forward (come to me), drive pin 0&5 to some power
// to backward (leave me), drive pin 2&4 to some power
// forward
digitalWrite(2, HIGH);
digitalWrite(5, LOW);
softPwmWrite(0, 1);
softPwmWrite(4, 1);
delay(5000);
// backward
digitalWrite(2, LOW);
digitalWrite(5, HIGH);
softPwmWrite(0, 1);
softPwmWrite(4, 1);
delay(5000);
// stop
softPwmWrite(0, 0);
softPwmWrite(4, 0);
digitalWrite(3, LOW);
digitalWrite(6, LOW);
delay(1000);
}
int main (int argc, char **argv) {
if (wiringPiSetup () == -1) {
fprintf (stdout, "oops: %s\n", strerror (errno)) ;
return 1;
}
if (argc < 1) {
fprintf(stdout, "Usage: picolord /path/to/socket");
}
softPwmCreate (PIN_RED, RED_MIN, RED_MAX);
softPwmCreate (PIN_GREEN, GREEN_MIN, GREEN_MAX);
softPwmCreate (PIN_BLUE, BLUE_MIN, BLUE_MAX);
struct sockaddr_un server;
sock = socket(AF_UNIX, SOCK_STREAM, 0);
if (sock < 0) {
perror("picolord: Error opening stream socket");
exit(1);
}
fcntl(sock, F_SETFL, O_NONBLOCK);
server.sun_family = AF_UNIX;
strcpy(server.sun_path, argv[1]);
if (bind(sock, (struct sockaddr *) &server, sizeof(struct sockaddr_un))) {
perror("picolord: Error binding stream socket");
exit(1);
}
listen(sock, 5);
// Catch kill signals and ctrl+c
signal(SIGINT, intHandler);
signal(SIGKILL, intHandler);
signal(SIGHUP, intHandler);
while (keeprunning) {
read_socket();
update_color();
delay(delay_time);
}
set_color(0,0,0);
delay(100);
if (sock) close(sock);
unlink(argv[1]);
return 0;
}
DCMotor::DCMotor(int mA, int mB)
{
motorA = mA;
motorB = mB;
int test1, test2;
pinMode(motorA, PWM_OUTPUT); //setup hardware pwm
pinMode(motorB, PWM_OUTPUT); //setup hardware pwm
test1 = softPwmCreate (motorA, 100, 100); // (pin, initial value, pwm range)
test2 = softPwmCreate (motorB, 100, 100); // (pin, initial value, pwm range)
if (test1 != 0 || test2 != 0)
{
printf("Warning: error creating soft PWM! \n");
exit(1);
}
}
/*左モータ情報の初期化*/
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;
}
/*
* setPin()
* @function : Selecting Pin number using softpwm output
* @param : pinNum, pin using pwm gpio
* @return : -1 Error, 1 OK
* @detail : 공통부분. 모터 사용 핀넘버 설정.
*/
int Motor::setPin(int pinNum)
{
pin = pinNum;
if(softPwmCreate(pin, 0, 2500) < 0)
return -1;
return 1;
}
void main() {
char *df = "/dev/input/js0";
struct ps3ctls ps3dat;
wiringPiSetup();
mcp23017Setup (BASE, 0x20) ;
softPwmCreate(100,0,100); // GP100 start-0 10ms
softPwmCreate(101,0,100); // GP101 start-0 10ms
if(!(ps3c_init(&ps3dat, df))) {
do {
if (ps3c_test(&ps3dat) < 0) break;
} while (!(ps3c_input(&ps3dat)));
ps3c_exit(&ps3dat);
}
}
int main(void) {
wiringPiSetupGpio();
softPwmCreate(ledPin, 0, 100);
for (int x = 0; x < 5; x++) {
softPwmWrite(ledPin, pins[x]);
delay(500);
}
return 0;
}
int main ()
{
int i, j ;
char buf [80] ;
if (wiringPiSetup () == -1)
{
fprintf (stdout, "oops: %s\n", strerror (errno)) ;
return 1 ;
}
for (i = 0 ; i < NUM_LEDS ; ++i)
{
softPwmCreate (ledMap [i], 0, RANGE) ;
printf ("%3d, %3d, %3d\n", i, ledMap [i], values [i]) ;
}
fgets (buf, 80, stdin) ;
// Bring all up one by one:
for (i = 0 ; i < NUM_LEDS ; ++i)
for (j = 0 ; j <= 100 ; ++j)
{
softPwmWrite (ledMap [i], j) ;
delay (10) ;
}
fgets (buf, 80, stdin) ;
// Down fast
for (i = 100 ; i > 0 ; --i)
{
for (j = 0 ; j < NUM_LEDS ; ++j)
softPwmWrite (ledMap [j], i) ;
delay (10) ;
}
fgets (buf, 80, stdin) ;
for (;;)
{
for (i = 0 ; i < NUM_LEDS ; ++i)
softPwmWrite (ledMap [i], values [i]) ;
delay (50) ;
i = values [0] ;
for (j = 0 ; j < NUM_LEDS - 1 ; ++j)
values [j] = values [j + 1] ;
values [NUM_LEDS - 1] = i ;
}
}
void configureIO(int pinToConfigure, int value) {
if(value == ANALOG_OUTPUT) {
softPwmCreate(pinToConfigure, value, 100);
}
else if(value == DIGITAL_OUTPUT) {
pinMode(pinToConfigure, value);
}
else if(value == INPUT) {
pinMode(pinToConfigure, value);
}
}
void init_servo()
{
if ((fd = serialOpen ("/dev/ttyAMA0", 9600)) < 0)
{
fprintf (stderr, "Unable to open serial device: %s\n", strerror (errno)) ;
return 1 ;
}
printf("init fd: %d",fd);
//inital Thread
int x = piThreadCreate(myThread);
if(x!=0)
{
printf("No ko the bat dau\n");
}
if (wiringPiSetup () == -1)
{
fprintf (stdout, "Unable to start wiringPi: %s\n", strerror (errno)) ;
return 1 ;
}
// initial 0 & 1 pin PWM are output
pinMode (1, OUTPUT) ; // camera go up/down
pinMode(0,OUTPUT); //camera turn lerf/right
// pull 2 pin PWM to GND
digitalWrite (1, LOW) ;
digitalWrite(0, LOW);
//setup 2 pin is PWM
softPwmCreate(0,0,200);
softPwmCreate(1,0,200);
//setup the location defaults
i_count = 0;
softPwmWrite(1,175+posHor);
//delay(100);
softPwmWrite(0,175+posVer);
//delay(1000);
}
void initRGBLEDs() {
softPwmCreate (PIN_Eye1_R, 100, 100);
softPwmCreate (PIN_Eye1_G, 100, 100);
softPwmCreate (PIN_Eye1_B, 100, 100);
softPwmCreate (PIN_Eye2_R, 100, 100);
softPwmCreate (PIN_Eye2_G, 100, 100);
softPwmCreate (PIN_Eye2_B, 100, 100);
}
int main ()
{
int i, j ;
char buf [80] ;
wiringPiSetup () ;
for (i = 0 ; i < NUM_LEDS ; ++i)
{
softPwmCreate (ledMap [i], 0, RANGE) ;
printf ("%3d, %3d, %3d\n", i, ledMap [i], values [i]) ;
}
fgets (buf, 80, stdin) ;
// Bring all up one by one:
for (i = 0 ; i < NUM_LEDS ; ++i)
for (j = 0 ; j <= 100 ; ++j)
{
softPwmWrite (ledMap [i], j) ;
delay (10) ;
}
fgets (buf, 80, stdin) ;
// All Down
for (i = 100 ; i > 0 ; --i)
{
for (j = 0 ; j < NUM_LEDS ; ++j)
softPwmWrite (ledMap [j], i) ;
delay (10) ;
}
fgets (buf, 80, stdin) ;
for (;;)
{
for (i = 0 ; i < NUM_LEDS ; ++i)
softPwmWrite (ledMap [i], values [i]) ;
delay (50) ;
i = values [0] ;
for (j = 0 ; j < NUM_LEDS - 1 ; ++j)
values [j] = values [j + 1] ;
values [NUM_LEDS - 1] = i ;
}
}
motor_dc::motor_dc(unsigned char _Pin_EN, unsigned char _Pin_C1, unsigned char _Pin_C2){
Pin_EN = _Pin_EN;
Pin_C1 = _Pin_C1;
Pin_C2 = _Pin_C2;
vel = 0;
pinMode(Pin_C1, OUTPUT);
pinMode(Pin_C2, OUTPUT);
softPwmCreate(_Pin_EN, 0, 100);
digitalWrite(Pin_C1, 0);
digitalWrite(Pin_C2, 0);
softPwmWrite(Pin_EN, vel);
}
int main()
{
wiringPiSetup();
pinMode(0,OUTPUT);
pinMode(2,INPUT);
pinMode(3,INPUT);
softPwmCreate(0,0,100);
wiringPiISR(2,INT_EDGE_BOTH,updateEncoder);
wiringPiISR(3,INT_EDGE_BOTH,updateEncoder);
softPwmWrite(0,100);
delay(2000);
isr=0;
pomiar1=millis();
softPwmWrite(0,100);
delay(12000);
pomiar2=millis();
softPwmWrite(0,0);
softPwmWrite(0,100);
delay(3000);
wynik=pomiar2-pomiar1;
printf("Pomiar czasu: %i\n",wynik);
delay(1000);
predkosc=(isr*0.000119047)*(60000/wynik);
printf("Predkosc: %f\n",predkosc);
return;
}
