|
SInce its an arduino shield, have you tried just porting the arduino code to your opi? change the code or pin assignments to match your setup and use wiringOPi. Got all this from: https://www.raspberrypi.org/forums/viewtopic.php?t=16118
- #include <stdio.h>
- #include <errno.h>
- #include <string.h>
- #include <wiringPi.h>
- #include <softPwm.h>
- typedef unsigned char uint8_t;
- #define _BV(bit) (1 << (bit))
- #define MOTORLATCH 0
- #define MOTORCLK 1
- #define MOTORENABLE 2
- #define MOTORDATA 3
- #define MOTOR_1_PWM 4
- #define MOTOR_2_PWM 5
- #define MOTOR1_A 2
- #define MOTOR1_B 3
- #define MOTOR2_A 1
- #define MOTOR2_B 4
- #define MOTOR4_A 0
- #define MOTOR4_B 6
- #define MOTOR3_A 5
- #define MOTOR3_B 7
- #define FORWARD 1
- #define BACKWARD 2
- #define BRAKE 3
- #define RELEASE 4
- #define RANGE 100
- #define DC_MOTORS 4
- static unsigned char latch_state;
- void latch_tx(void)
- {
- unsigned char i;
- digitalWrite(MOTORLATCH, LOW);
- digitalWrite(MOTORDATA, LOW);
- for (i=0; i<8; i++)
- {
- digitalWrite(MOTORCLK, LOW);
- if (latch_state & _BV(7-i))
- {
- digitalWrite(MOTORDATA, HIGH);
- }
- else
- {
- digitalWrite(MOTORDATA, LOW);
- }
- digitalWrite(MOTORCLK, HIGH);
- }
- digitalWrite(MOTORLATCH, HIGH);
- }
- void enable(void)
- {
- pinMode(MOTORLATCH, OUTPUT);
- pinMode(MOTORENABLE, OUTPUT);
- pinMode(MOTORDATA, OUTPUT);
- pinMode(MOTORCLK, OUTPUT);
- latch_state = 0;
- latch_tx();
- digitalWrite(MOTORENABLE, LOW);
- }
- typedef struct motorInf_t
- {
- uint8_t pwmfreq;
- };
- motorInt_t motorInf[DC_MOTORS];
- void DCMotorInit(uint8_t num, uint8_t freq)
- {
- motorInf[num].pwmfreq = freq;
- enable();
- switch (num)
- {
- case 1:
- latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B);
- latch_tx();
- initPWM(1, freq);
- break;
- case 2:
- latch_state &= ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B);
- latch_tx();
- initPWM(2, freq);
- break;
- case 3:
- latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B);
- latch_tx();
- initPWM(3, freq);
- break;
- case 4:
- latch_state &= ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B);
- latch_tx();
- initPWM(4, freq);
- break;
- }
- }
- int motorMap [] = {MOTOR_1_PWM, MOTOR_2_PWM};
- void setPWM(uint8_t num, int freq)
- {
- softPwmWrite (motorMap [num], freq) ;
- }
- void initPWM(uint8_t num, int freq)
- {
- softPwmCreate (motorMap [num], 0, RANGE) ;
- setPWM(num, freq);
- }
- void DCMotorRun(uint8_t motornum, uint8_t cmd)
- {
- uint8_t a, b;
- switch (motornum)
- {
- case 1:
- a = MOTOR1_A; b = MOTOR1_B;
- break;
- case 2:
- a = MOTOR2_A; b = MOTOR2_B;
- break;
- case 3:
- a = MOTOR3_A; b = MOTOR3_B;
- break;
- case 4:
- a = MOTOR4_A; b = MOTOR4_B;
- break;
- default:
- return;
- }
-
- switch (cmd)
- {
- case FORWARD:
- latch_state |= _BV(a);
- latch_state &= ~_BV(b);
- latch_tx();
- break;
- case BACKWARD:
- latch_state &= ~_BV(a);
- latch_state |= _BV(b);
- latch_tx();
- break;
- case RELEASE:
- latch_state &= ~_BV(a);
- latch_state &= ~_BV(b);
- latch_tx();
- break;
- }
- }
- void DCMotorSetSpeed(uint8_t motornum, int speed)
- {
- switch (motornum)
- {
- case 1:
- setPWM(1, speed);
- break;
- case 2:
- setPWM(2, speed);
- break;
- case 3:
- setPWM(3, speed);
- break;
- case 4:
- setPWM(4, speed);
- break;
- }
- }
- int main ()
- {
- int i, j ;
- if (wiringPiSetup () == -1)
- {
- fprintf (stdout, "oops: %s\n", strerror (errno)) ;
- return 1 ;
- }
- for (i = 0 ; i < NUM_MOTORS ; ++i)
- {
- softPwmCreate (motorMap [i], 0, RANGE) ;
- }
- // Bring all up in stages 25%, 50%, 75% 100%
- for (j = 25 ; j <= RANGE ; j+=25)
- {
- printf("pwm %d%%\n", j);
- for (i = 0 ; i < NUM_MOTORS ; ++i)
- {
- softPwmWrite (motorMap [i], j) ;
- }
- delay(6000);
- }
- // Bring all up in stages 25%, 50%, 75% 100%
- for (j = RANGE-25 ; j >= 0; j-=25)
- {
- printf("pwm %d%%\n", j);
- for (i = 0 ; i < NUM_MOTORS ; ++i)
- {
- softPwmWrite (motorMap [i], j) ;
- }
- delay(6000);
- }
- return 0;
- }
Copy code |
|