Working version
This commit is contained in:
commit
6727b4fdc9
|
|
@ -0,0 +1,5 @@
|
|||
# This code depends on make tool being used
|
||||
DEPFILES=$(wildcard $(addsuffix .d, ${OBJECTFILES} ${TESTOBJECTFILES}))
|
||||
ifneq (${DEPFILES},)
|
||||
include ${DEPFILES}
|
||||
endif
|
||||
|
|
@ -0,0 +1,17 @@
|
|||
bt_car: main.o joystick.o rpiPWM1.o rpiServo.o
|
||||
g++ -o bt_car joystick.o main.o rpiPWM1.o rpiServo.o -lwiringPi -lpthread
|
||||
|
||||
main.o: main.cpp
|
||||
g++ main.cpp -c -o main.o
|
||||
|
||||
joystick.o: joystick.hh joystick.cc
|
||||
g++ joystick.cc -c -o joystick.o
|
||||
|
||||
rpiPWM1.o: rpiPWM1.h rpiPWM1.cpp
|
||||
g++ rpiPWM1.cpp -c -o rpiPWM1.o
|
||||
|
||||
rpiServo.o: rpiServo.h rpiServo.cpp
|
||||
g++ rpiServo.cpp -c -o rpiServo.o
|
||||
|
||||
clean:
|
||||
rm *.o bt_car
|
||||
|
|
@ -0,0 +1,83 @@
|
|||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
//
|
||||
// Copyright Drew Noakes 2013-2016
|
||||
|
||||
#include "joystick.hh"
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <sys/stat.h>
|
||||
#include <fcntl.h>
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
#include <sstream>
|
||||
#include "unistd.h"
|
||||
|
||||
Joystick::Joystick()
|
||||
{
|
||||
openPath("/dev/input/js0");
|
||||
}
|
||||
|
||||
Joystick::Joystick(int joystickNumber)
|
||||
{
|
||||
std::stringstream sstm;
|
||||
sstm << "/dev/input/js" << joystickNumber;
|
||||
openPath(sstm.str());
|
||||
}
|
||||
|
||||
Joystick::Joystick(std::string devicePath)
|
||||
{
|
||||
openPath(devicePath);
|
||||
}
|
||||
|
||||
Joystick::Joystick(std::string devicePath, bool blocking)
|
||||
{
|
||||
openPath(devicePath, blocking);
|
||||
}
|
||||
|
||||
void Joystick::openPath(std::string devicePath, bool blocking)
|
||||
{
|
||||
// Open the device using either blocking or non-blocking
|
||||
_fd = open(devicePath.c_str(), blocking ? O_RDONLY : O_RDONLY | O_NONBLOCK);
|
||||
}
|
||||
|
||||
bool Joystick::sample(JoystickEvent* event)
|
||||
{
|
||||
int bytes = read(_fd, event, sizeof(*event));
|
||||
|
||||
if (bytes == -1)
|
||||
return false;
|
||||
|
||||
// NOTE if this condition is not met, we're probably out of sync and this
|
||||
// Joystick instance is likely unusable
|
||||
return bytes == sizeof(*event);
|
||||
}
|
||||
|
||||
bool Joystick::isFound()
|
||||
{
|
||||
return _fd >= 0;
|
||||
}
|
||||
|
||||
Joystick::~Joystick()
|
||||
{
|
||||
close(_fd);
|
||||
}
|
||||
|
||||
std::ostream& operator<<(std::ostream& os, const JoystickEvent& e)
|
||||
{
|
||||
os << "type=" << static_cast<int>(e.type)
|
||||
<< " number=" << static_cast<int>(e.number)
|
||||
<< " value=" << static_cast<int>(e.value);
|
||||
return os;
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,154 @@
|
|||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
//
|
||||
// Copyright Drew Noakes 2013-2016
|
||||
|
||||
#ifndef __JOYSTICK_HH__
|
||||
#define __JOYSTICK_HH__
|
||||
|
||||
#include <string>
|
||||
#include <iostream>
|
||||
|
||||
#define JS_EVENT_BUTTON 0x01 // button pressed/released
|
||||
#define JS_EVENT_AXIS 0x02 // joystick moved
|
||||
#define JS_EVENT_INIT 0x80 // initial state of device
|
||||
|
||||
/**
|
||||
* Encapsulates all data relevant to a sampled joystick event.
|
||||
*/
|
||||
class JoystickEvent
|
||||
{
|
||||
public:
|
||||
/** Minimum value of axes range */
|
||||
static const short MIN_AXES_VALUE = -32768;
|
||||
|
||||
/** Maximum value of axes range */
|
||||
static const short MAX_AXES_VALUE = 32767;
|
||||
|
||||
/**
|
||||
* The timestamp of the event, in milliseconds.
|
||||
*/
|
||||
unsigned int time;
|
||||
|
||||
/**
|
||||
* The value associated with this joystick event.
|
||||
* For buttons this will be either 1 (down) or 0 (up).
|
||||
* For axes, this will range between MIN_AXES_VALUE and MAX_AXES_VALUE.
|
||||
*/
|
||||
short value;
|
||||
|
||||
/**
|
||||
* The event type.
|
||||
*/
|
||||
unsigned char type;
|
||||
|
||||
/**
|
||||
* The axis/button number.
|
||||
*/
|
||||
unsigned char number;
|
||||
|
||||
/**
|
||||
* Returns true if this event is the result of a button press.
|
||||
*/
|
||||
bool isButton()
|
||||
{
|
||||
return (type & JS_EVENT_BUTTON) != 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns true if this event is the result of an axis movement.
|
||||
*/
|
||||
bool isAxis()
|
||||
{
|
||||
return (type & JS_EVENT_AXIS) != 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns true if this event is part of the initial state obtained when
|
||||
* the joystick is first connected to.
|
||||
*/
|
||||
bool isInitialState()
|
||||
{
|
||||
return (type & JS_EVENT_INIT) != 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* The ostream inserter needs to be a friend so it can access the
|
||||
* internal data structures.
|
||||
*/
|
||||
friend std::ostream& operator<<(std::ostream& os, const JoystickEvent& e);
|
||||
};
|
||||
|
||||
/**
|
||||
* Stream insertion function so you can do this:
|
||||
* cout << event << endl;
|
||||
*/
|
||||
std::ostream& operator<<(std::ostream& os, const JoystickEvent& e);
|
||||
|
||||
/**
|
||||
* Represents a joystick device. Allows data to be sampled from it.
|
||||
*/
|
||||
class Joystick
|
||||
{
|
||||
private:
|
||||
void openPath(std::string devicePath, bool blocking=false);
|
||||
|
||||
int _fd;
|
||||
|
||||
public:
|
||||
~Joystick();
|
||||
|
||||
/**
|
||||
* Initialises an instance for the first joystick: /dev/input/js0
|
||||
*/
|
||||
Joystick();
|
||||
|
||||
/**
|
||||
* Initialises an instance for the joystick with the specified,
|
||||
* zero-indexed number.
|
||||
*/
|
||||
Joystick(int joystickNumber);
|
||||
|
||||
/**
|
||||
* Initialises an instance for the joystick device specified.
|
||||
*/
|
||||
Joystick(std::string devicePath);
|
||||
|
||||
/**
|
||||
* Joystick objects cannot be copied
|
||||
*/
|
||||
Joystick(Joystick const&) = delete;
|
||||
|
||||
/**
|
||||
* Joystick objects can be moved
|
||||
*/
|
||||
Joystick(Joystick &&) = default;
|
||||
|
||||
/**
|
||||
* Initialises an instance for the joystick device specified and provide
|
||||
* the option of blocking I/O.
|
||||
*/
|
||||
Joystick(std::string devicePath, bool blocking);
|
||||
|
||||
/**
|
||||
* Returns true if the joystick was found and may be used, otherwise false.
|
||||
*/
|
||||
bool isFound();
|
||||
|
||||
/**
|
||||
* Attempts to populate the provided JoystickEvent instance with data
|
||||
* from the joystick. Returns true if data is available, otherwise false.
|
||||
*/
|
||||
bool sample(JoystickEvent* event);
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,109 @@
|
|||
/*
|
||||
* To change this license header, choose License Headers in Project Properties.
|
||||
* To change this template file, choose Tools | Templates
|
||||
* and open the template in the editor.
|
||||
*/
|
||||
|
||||
/*
|
||||
* File: main.cpp
|
||||
* Author: pi
|
||||
*
|
||||
* Created on 2018. május 14., 10:10
|
||||
*/
|
||||
|
||||
#include <cstdlib>
|
||||
#include "joystick.hh"
|
||||
#include <unistd.h>
|
||||
#include <wiringPi.h>
|
||||
#include <fcntl.h>
|
||||
//#include <softPwm.h>
|
||||
//#include "rpiServo.h"
|
||||
|
||||
#define PIN0 0
|
||||
#define PIN1 2
|
||||
#define PIN2 3
|
||||
#define PWM_RANGE 100
|
||||
#define SERVO_PWM_RANGE 50
|
||||
#define PWM_INIT_VALUE 0
|
||||
|
||||
using namespace std;
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
wiringPiSetup();
|
||||
|
||||
char cmd[100];
|
||||
|
||||
//pinMode(0, PWM_OUTPUT);
|
||||
int pwm0, pwm1, pwm2;
|
||||
pwm0 = 0;
|
||||
pwm1 = 0;
|
||||
pwm2 = 0;
|
||||
|
||||
//int pin0 = softPwmCreate(PIN0, PWM_INIT_VALUE, PWM_RANGE);
|
||||
//int pin1 = softPwmCreate(PIN1, PWM_INIT_VALUE, PWM_RANGE);
|
||||
//int pin2 = softPwmCreate(PIN2, PWM_INIT_VALUE, SERVO_PWM_RANGE);
|
||||
|
||||
//softPwmWrite(0, 256);
|
||||
|
||||
//pwmWrite(PIN0, 128);
|
||||
|
||||
Joystick joy("/dev/input/js0");
|
||||
//rpiServo servo;
|
||||
//servo.setAngle(30);
|
||||
|
||||
while (true)
|
||||
{
|
||||
// Restrict rate
|
||||
usleep(100);
|
||||
|
||||
// Attempt to sample an event from the joystick
|
||||
JoystickEvent event;
|
||||
if (joy.sample(&event))
|
||||
{
|
||||
if (event.isButton())
|
||||
{
|
||||
printf("Button %u is %s\n", event.number, event.value == 0 ? "up" : "down");
|
||||
}
|
||||
else if (event.isAxis())
|
||||
{
|
||||
printf("Axis %u is at position %d\n", event.number, event.value);
|
||||
if(event.number == 4) //forward
|
||||
{
|
||||
if(pwm1 == 0)
|
||||
{
|
||||
pwm0 = ((event.value+32768)/2620);
|
||||
//softPwmWrite(PIN0, pwm0);
|
||||
snprintf(cmd, 100, "echo 1=%d > /dev/servoblaster",-pwm0+150);
|
||||
system(cmd);
|
||||
}
|
||||
}
|
||||
if(event.number == 5) //backward
|
||||
{
|
||||
if(pwm0 == 0)
|
||||
{
|
||||
pwm1 = ((event.value+32768)/2620);
|
||||
//softPwmWrite(PIN2, pwm1);
|
||||
snprintf(cmd, 100, "echo 1=%d > /dev/servoblaster",pwm1+150);
|
||||
system(cmd);
|
||||
}
|
||||
}
|
||||
if(event.number == 0) //servo
|
||||
{
|
||||
pwm2 = (event.value/1092)+152;
|
||||
//fprintf(fp, "2=%d",pwm2);
|
||||
//fflush(fp);
|
||||
snprintf(cmd, 100, "echo 2=%d > /dev/servoblaster",pwm2);
|
||||
system(cmd);
|
||||
std::cout << "PWM: " << pwm2 << std::endl;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,431 @@
|
|||
#include "rpiPWM1.h"
|
||||
|
||||
//Need to do this to be able to access these constants
|
||||
//Private constants
|
||||
const int rpiPWM1::BCM2708_PERI_BASE;
|
||||
const int rpiPWM1::PWM_BASE;/* PWM controller */
|
||||
const int rpiPWM1::CLOCK_BASE; /* Clock controller */
|
||||
const int rpiPWM1::GPIO_BASE; /* GPIO controller */
|
||||
const int rpiPWM1::PWM_CTL ;
|
||||
const int rpiPWM1::PWM_RNG1;
|
||||
const int rpiPWM1::PWM_DAT1;
|
||||
const int rpiPWM1::PWMCLK_CNTL;
|
||||
const int rpiPWM1::PWMCLK_DIV;
|
||||
const int rpiPWM1::BLOCK_SIZE;
|
||||
//Public constants
|
||||
const int rpiPWM1::PWMMODE;
|
||||
const int rpiPWM1::MSMODE;
|
||||
const int rpiPWM1::ERRFREQ;
|
||||
const int rpiPWM1::ERRCOUNT;
|
||||
const int rpiPWM1::ERRDUTY;
|
||||
const int rpiPWM1::ERRMODE;
|
||||
|
||||
|
||||
/***********************************************************************
|
||||
* rpiPWM1::rpiPWM1()
|
||||
* This is the Default constructor. First, it mmaps the registers in
|
||||
* Physical memory responsible for configuring GPIO, PWM and the PWM clock.
|
||||
* It then sets the frequency to 1KHz, PWM resolution to 256, duty
|
||||
* cycle to 50% & pwm mode to 'PWMMODE'
|
||||
* It then calls configPWM1Pin() to configure GPIO18 to ALT5 to allow it to
|
||||
* output PWM1 waveforms.
|
||||
* Finally configPWM1() is called to configure the PWM1 peripheral
|
||||
***********************************************************************/
|
||||
rpiPWM1::rpiPWM1()
|
||||
{
|
||||
this->clk = mapRegAddr(CLOCK_BASE);// map PWM clock registers into memory
|
||||
this->pwm = mapRegAddr(PWM_BASE); //map PWM registers into memory
|
||||
this->gpio = mapRegAddr(GPIO_BASE);// map GPIO registers into memory
|
||||
this->frequency = 1000.0; // set frequency
|
||||
this->counts = 256; //set PWM resolution
|
||||
this->dutyCycle = 50.0; //set duty cycle
|
||||
this->mode = PWMMODE; // set pwm mode
|
||||
configPWM1Pin(); //configure GPIO18 to ALT15 (PWM output)
|
||||
configPWM1(); // configure PWM1
|
||||
}
|
||||
|
||||
/***********************************************************************
|
||||
* rpiPWM1::rpiPWM1(double Hz, unsigned int cnts, double duty, unsigned int m)
|
||||
* This is the overloaded constructor. First, it mmaps the registers in
|
||||
* Physical memory responsible for configuring GPIO, PWM and the PWM clock.
|
||||
* It then sets the frequency, PWM resolution, duty cycle & pwm mode as
|
||||
* per the parameters provided.
|
||||
*
|
||||
* It then calls configPWM1Pin() to configure GPIO18 to ALT5 to allow it to
|
||||
* output PWM1 waveforms.
|
||||
* Finally configPWM1() is called to configure the PWM1 peripheral
|
||||
* Parameters: - Hz (double) - Frequency
|
||||
* - cnts (unsigned int) - PWM resolution (counts)
|
||||
* - duty (double) - Duty Cycle as a percentage
|
||||
* - m (int) - PWM mode (can be either 1 for PWMMODE (rpiPWM1::PWMMODE)
|
||||
* or 2 for MSMODE (rpiPWM1::MSMODE)
|
||||
***********************************************************************/
|
||||
rpiPWM1::rpiPWM1(double Hz, unsigned int cnts, double duty, int m)
|
||||
{
|
||||
this->clk = mapRegAddr(CLOCK_BASE);
|
||||
this->gpio = mapRegAddr(GPIO_BASE);
|
||||
this->pwm = mapRegAddr(PWM_BASE);
|
||||
|
||||
if( (cnts < 0) || (cnts > UINT_MAX) ) {
|
||||
printf("counts value must be between 0-%d\n",UINT_MAX);
|
||||
exit(1);
|
||||
}
|
||||
|
||||
if ((Hz < 1e-5) || (Hz > 19200000.0f)){
|
||||
printf("frequency value must be between 0-19200000\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
if( (duty < 1e-5) || (duty> 99.99999) ) {
|
||||
printf("dutyCycle value must be between 0-99.99999\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
if( (m != PWMMODE) && (m != MSMODE) ) {
|
||||
printf("mode must be either PWMMODE(1) or MSMODE(2)\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
this->frequency = Hz;
|
||||
this->counts = cnts;
|
||||
this->dutyCycle = duty;
|
||||
this->mode = m;
|
||||
configPWM1Pin();
|
||||
configPWM1();
|
||||
}
|
||||
|
||||
/***********************************************************************
|
||||
* rpiPWM1::~rpiPWM1()
|
||||
* Destructor - Puts all Peripheral registers in their original (reset state)
|
||||
* and then unmaps the portions of memory containing to register addresses
|
||||
* for the PWM clock, PWM and GPIO peripherals
|
||||
***********************************************************************/
|
||||
rpiPWM1::~rpiPWM1(){
|
||||
|
||||
|
||||
//lets put the PWM peripheral registers in their original state
|
||||
*(pwm + PWM_CTL) = 0;
|
||||
*(pwm + PWM_RNG1) = 0x20;
|
||||
*(pwm + PWM_DAT1) = 0;
|
||||
// unmap the memory block containing PWM registers
|
||||
if(munmap((void*)pwm, BLOCK_SIZE) < 0){
|
||||
perror("munmap (pwm) failed");
|
||||
exit(1);
|
||||
}
|
||||
//lets put the PWM Clock peripheral registers in their original state
|
||||
//kill PWM clock
|
||||
*(clk + PWMCLK_CNTL) = 0x5A000000 | (1 << 5);
|
||||
usleep(10);
|
||||
|
||||
// wait until busy flag is set
|
||||
while ( (*(clk + PWMCLK_CNTL)) & 0x00000080){}
|
||||
|
||||
//reset divisor
|
||||
*(clk + PWMCLK_DIV) = 0x5A000000;
|
||||
usleep(10);
|
||||
|
||||
// source=osc and enable clock
|
||||
*(clk + PWMCLK_CNTL) = 0x5A000011;
|
||||
|
||||
// unmap the memory block containing PWM Clock registers
|
||||
if(munmap((void*)clk, BLOCK_SIZE) < 0){
|
||||
perror("munmap (clk) failed");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
//lets put the GPIO peripheral registers in their original state
|
||||
//first put it in input mode (default)
|
||||
//taken from #define INP_GPIO(g) *(gpio+((g)/10)) &= ~(7<<(((g)%10)*3))
|
||||
*(gpio+1) &= ~(7 << 24);
|
||||
//then munmap
|
||||
if(munmap((void*)gpio, BLOCK_SIZE) < 0){
|
||||
perror("munmap (gpio) failed");
|
||||
exit(1);
|
||||
}
|
||||
}
|
||||
|
||||
/***********************************************************************
|
||||
* unsigned int rpiPWM1::setFrequency(const double &hz)
|
||||
* This function sets the PWM frequency and then reinitializes the PWM1
|
||||
* peripheral to update the frequency. The function performs a check to
|
||||
* ensure that the PWM frequency is between 0 & 19.2MHz
|
||||
* Parameters: hz (double) - Frequency in Hz
|
||||
* Return Value: 0 if successful or rpiPWM1::ERRFREQ if frequency
|
||||
* parameter is invalid
|
||||
***********************************************************************/
|
||||
unsigned int rpiPWM1::setFrequency(const double &hz){
|
||||
unsigned int retVal = 0;
|
||||
if (hz < 1e-5 || hz > 19200000.0f){ // make sure that Frequency is valid
|
||||
retVal = ERRFREQ; //if not return error code
|
||||
}
|
||||
else{
|
||||
this->frequency = hz;
|
||||
configPWM1();
|
||||
}
|
||||
return retVal; // return 0 for success.....
|
||||
}
|
||||
|
||||
|
||||
/***********************************************************************
|
||||
* unsigned int rpiPWM1::setCounts(const int &cnts)
|
||||
* This function sets the PWM resolution and then reinitializes the PWM1
|
||||
* peripheral to update the PWM resolution (counts). The function performs a check to
|
||||
* ensure that the PWM resolution is between 0 & UINT_MAX (its a 32-bit register)
|
||||
* Parameters: cnts (unsigned int) - counts
|
||||
* Return Value: 0 if successful or rpiPWM1::ERRCOUNT if count value is invalid
|
||||
***********************************************************************/
|
||||
unsigned int rpiPWM1::setCounts(const unsigned int &cnts){
|
||||
unsigned int retVal = 0;
|
||||
|
||||
if( (cnts < 0) || (cnts > UINT_MAX) ) {
|
||||
retVal = ERRCOUNT;
|
||||
}
|
||||
else{
|
||||
this->counts = cnts;
|
||||
configPWM1();
|
||||
}
|
||||
return retVal;
|
||||
}
|
||||
|
||||
/***********************************************************************
|
||||
* unsigned int rpiPWM1::setDutyCycle(const double &duty)
|
||||
* This function sets the PWM DutyCycle while the PWM peripheral is running.
|
||||
* The function performs a check to ensure that the PWM Duty Cycle is between
|
||||
* 0 & 99.99999 %
|
||||
* Parameters: duty (double) - Duty Cycle in %
|
||||
* Return Value: 0 if successful or rpiPWM1::ERRDUTY if Duty cycle is invalid
|
||||
****************************************************************************/
|
||||
unsigned int rpiPWM1::setDutyCycle(const double &duty){
|
||||
unsigned int bitCount = 0;
|
||||
unsigned int retVal = 0;
|
||||
|
||||
if( (duty < 1e-5) || (duty > 99.99999) ) {
|
||||
retVal = ERRDUTY;
|
||||
}
|
||||
else {
|
||||
this->dutyCycle = duty;
|
||||
bitCount = (int) ((this->dutyCycle/100.0) * this->counts);
|
||||
*(pwm + PWM_DAT1) = bitCount;
|
||||
}
|
||||
|
||||
return retVal;
|
||||
}
|
||||
|
||||
/***********************************************************************
|
||||
* unsigned int rpiPWM1::setDutyCycleForce(const double &duty, unsigned int &m)
|
||||
* This function firsts stops the PWM1 peripheral, sets the PWM DutyCycle
|
||||
* and the PWM mode and then re-enables the PWM1 peripheral in the new mode
|
||||
* The function performs a check to ensure that the PWM Duty Cycle is between
|
||||
* 0 & 99.99999 % and that an appropriate mode is selected.
|
||||
*
|
||||
* Parameters: duty (double) - Duty Cycle in %
|
||||
* m (int) - pwm mode (rpiPWM1::PWMMODE or rpiPWM1::MSMODE)
|
||||
* Return Value: 0 if successful or rpiPWM1::ERRDUTY if Duty cycle is invalid
|
||||
*******************************************************************************/
|
||||
unsigned int rpiPWM1::setDutyCycleForce(const double &duty, const int &m){
|
||||
int retVal = 0;
|
||||
if( (m != PWMMODE) && (m != MSMODE) ) {
|
||||
retVal = ERRMODE;
|
||||
}
|
||||
else if( (duty < 1e-5) || (duty > 99.99999) ) {
|
||||
retVal = ERRDUTY;
|
||||
}
|
||||
else{
|
||||
this->mode = m;
|
||||
this->dutyCycle = duty;
|
||||
// disable PWM & start from a clean slate
|
||||
*(pwm + PWM_CTL) = 0;
|
||||
// needs some time until the PWM module gets disabled, without the delay the PWM module crashs
|
||||
usleep(10);
|
||||
// set the number of counts that constitute a period
|
||||
*(pwm + PWM_RNG1) = this->counts;
|
||||
//set duty cycle
|
||||
*(pwm + PWM_DAT1) = (int) ((this->dutyCycle/100.0) * this->counts);
|
||||
// start PWM1 in
|
||||
if(this->mode == PWMMODE) //PWM mode
|
||||
*(pwm + PWM_CTL) |= (1 << 0);
|
||||
else // M/S Mode
|
||||
*(pwm + PWM_CTL) |= ( (1 << 7) | (1 << 0) );
|
||||
}
|
||||
return retVal;
|
||||
}
|
||||
|
||||
/***********************************************************************
|
||||
* unsigned int rpiPWM1::setDutyCycleCount(unsigned int &dutyCycleCnts )
|
||||
* This function sets the PWM DutyCycle as a function of PWM resolution,
|
||||
* while the PWM peripheral is running. The function performs a check to
|
||||
* ensure that the PWM Duty Cycle count value is between 0 and count
|
||||
* Parameters: dutyCycleCnts (unsigned int) - Duty Cycle in counts
|
||||
* Return Value:0 if successful or rpiPWM1::ERRDUTY if Duty cycle is invalid
|
||||
***********************************************************************/
|
||||
unsigned int rpiPWM1::setDutyCycleCount(const unsigned int &dutyCycleCnts ){
|
||||
unsigned int retVal = 0;
|
||||
if( (dutyCycleCnts < 0) || ( dutyCycleCnts > this->counts )) {
|
||||
retVal = ERRDUTY;
|
||||
}
|
||||
else{
|
||||
this->dutyCycle = ((dutyCycleCnts * 1.0)/ this->counts) * 100.0;
|
||||
*(pwm + PWM_DAT1) = dutyCycleCnts;
|
||||
}
|
||||
return retVal;
|
||||
}
|
||||
|
||||
/***********************************************************************
|
||||
* unsigned int rpiPWM1::setMode(unsigned int &m)
|
||||
* This function sets the PWM mode. The function performs a check to
|
||||
* ensure that a valid PWM mode is requested.
|
||||
* Parameters: m (int) - pwm mode (rpiPWM1::PWMMODE or rpiPWM1::MSMODE)
|
||||
* Return Value: 0 if successful or rpiPWM1::ERRMODE if MODE is invalid
|
||||
********************************************************************************/
|
||||
unsigned int rpiPWM1::setMode(const int &m){
|
||||
unsigned int retVal = 0;
|
||||
if( (m != PWMMODE) && (m != MSMODE) ) {
|
||||
retVal = ERRMODE;
|
||||
}
|
||||
else{
|
||||
this->mode = m;
|
||||
setDutyCycleForce(this->dutyCycle, this->mode);
|
||||
}
|
||||
|
||||
return retVal;
|
||||
}
|
||||
|
||||
|
||||
/***********************************************************************
|
||||
*These are a bunch of 'getter' functions that enable the user
|
||||
* to access the PWM frequency, resolution, duty cycle and mode as well
|
||||
* as the PWM clock divisor value.
|
||||
*********************************************************************/
|
||||
double rpiPWM1::getFrequency() const{ return this->frequency;}
|
||||
int rpiPWM1::getCounts() const { return this->counts;}
|
||||
int rpiPWM1::getDivisor() const {return this->divisor;}
|
||||
double rpiPWM1::getDutyCycle() const {return this->dutyCycle;}
|
||||
int rpiPWM1::getMode() const{return this->mode;}
|
||||
|
||||
/***********************************************************************
|
||||
* volatile unsigned *rpiPWM1::mapRegAddr(unsigned long baseAddr)
|
||||
* This function maps a block (4KB) of physical memory into the memory of
|
||||
* the calling process. It enables a user space process to access registers
|
||||
* in physical memory directly without having to interact with in kernel side
|
||||
* code i.e. device drivers
|
||||
* Parameter - baseAddr (unsigned long) - this is the base address of a 4KB
|
||||
* block of physical memory that will be mapped into the user
|
||||
* space process memory.
|
||||
* Return Value - mapped pointer in process memory
|
||||
***********************************************************************/
|
||||
volatile unsigned *rpiPWM1::mapRegAddr(unsigned long baseAddr){
|
||||
int mem_fd = 0;
|
||||
void *regAddrMap = MAP_FAILED;
|
||||
|
||||
/* open /dev/mem.....need to run program as root i.e. use sudo or su */
|
||||
if (!mem_fd) {
|
||||
if ((mem_fd = open("/dev/mem", O_RDWR|O_SYNC) ) < 0) {
|
||||
perror("can't open /dev/mem");
|
||||
exit (1);
|
||||
}
|
||||
}
|
||||
|
||||
/* mmap IO */
|
||||
regAddrMap = mmap(
|
||||
NULL, //Any adddress in our space will do
|
||||
BLOCK_SIZE, //Map length
|
||||
PROT_READ|PROT_WRITE|PROT_EXEC,// Enable reading & writting to mapped memory
|
||||
MAP_SHARED|MAP_LOCKED, //Shared with other processes
|
||||
mem_fd, //File to map
|
||||
baseAddr //Offset to base address
|
||||
);
|
||||
|
||||
if (regAddrMap == MAP_FAILED) {
|
||||
perror("mmap error");
|
||||
close(mem_fd);
|
||||
exit (1);
|
||||
}
|
||||
|
||||
if(close(mem_fd) < 0){ //No need to keep mem_fd open after mmap
|
||||
//i.e. we can close /dev/mem
|
||||
perror("couldn't close /dev/mem file descriptor");
|
||||
exit(1);
|
||||
}
|
||||
return (volatile unsigned *)regAddrMap;
|
||||
}
|
||||
|
||||
|
||||
/***********************************************************************
|
||||
* void rpiPWM1::configPWM1Pin()
|
||||
* This function is responsible for putting GPIO18 in PWM mode by setting
|
||||
* its alternate function to ALT5. It firsts make the GPIO an input
|
||||
* (default state) and then switches to alternate function 5 (PWM mode)
|
||||
***********************************************************************/
|
||||
void rpiPWM1::configPWM1Pin()
|
||||
{
|
||||
|
||||
/*GPIO 18 in ALT5 mode for PWM0 */
|
||||
// Let's first set pin 18 to input
|
||||
//taken from #define INP_GPIO(g) *(gpio+((g)/10)) &= ~(7<<(((g)%10)*3))
|
||||
*(gpio+1) &= ~(7 << 24);
|
||||
//then set it to ALT5 function PWM0
|
||||
//taken from #define SET_GPIO_ALT(g,a) *(gpio+(((g)/10))) |= (((a)<=3?(a)+4:(a)==4?3:2)<<(((g)%10)*3))
|
||||
*(gpio+1) |= (2<<24);
|
||||
|
||||
}
|
||||
|
||||
|
||||
/***********************************************************************
|
||||
* void rpiPWM1::configPWM1()
|
||||
* This function configures the PWM1 peripheral.
|
||||
* - It stops the PWM clock
|
||||
* - Calculates an appropriate divisor value based on PWM Freq and resolution
|
||||
* - Writes this divisor value to the PWM clock
|
||||
* - Enables the PWM clock
|
||||
* - Disables the PWM peripheral
|
||||
* - Writes the PWM resolution and Duty Cycle to the appropriate registers
|
||||
* - Enables the PWM peripheral in the requested mode
|
||||
* *********************************************************************/
|
||||
void rpiPWM1::configPWM1(){
|
||||
|
||||
double period;
|
||||
double countDuration;
|
||||
|
||||
// stop clock and waiting for busy flag doesn't work, so kill clock
|
||||
*(clk + PWMCLK_CNTL) = 0x5A000000 | (1 << 5);
|
||||
usleep(10);
|
||||
|
||||
// wait until busy flag is set
|
||||
while ( (*(clk + PWMCLK_CNTL)) & 0x00000080){}
|
||||
|
||||
//calculate divisor value for PWM1 clock...base frequency is 19.2MHz
|
||||
period = 1.0/this->frequency;
|
||||
countDuration = period/(counts*1.0f);
|
||||
this->divisor = (int)(19200000.0f / (1.0/countDuration));
|
||||
|
||||
if( this->divisor < 0 || this->divisor > 4095 ) {
|
||||
printf("divisor value must be between 0-4095\n");
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
//set divisor
|
||||
*(clk + PWMCLK_DIV) = 0x5A000000 | (this->divisor << 12);
|
||||
|
||||
// source=osc and enable clock
|
||||
*(clk + PWMCLK_CNTL) = 0x5A000011;
|
||||
|
||||
// disable PWM & start from a clean slate
|
||||
*(pwm + PWM_CTL) = 0;
|
||||
|
||||
// needs some time until the PWM module gets disabled, without the delay the PWM module crashs
|
||||
usleep(10);
|
||||
|
||||
// set the number of counts that constitute a period with 0 for 20 milliseconds = 320 bits
|
||||
*(pwm + PWM_RNG1) = this->counts; usleep(10);
|
||||
|
||||
//set duty cycle
|
||||
*(pwm + PWM_DAT1) = (int) ((this->dutyCycle/100.0) * this->counts); usleep(10);
|
||||
|
||||
|
||||
// start PWM1 in
|
||||
if(this->mode == PWMMODE) //PWM mode
|
||||
*(pwm + PWM_CTL) |= (1 << 0);
|
||||
else // M/S Mode
|
||||
*(pwm + PWM_CTL) |= ( (1 << 7) | (1 << 0) );
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,177 @@
|
|||
#ifndef RPIPWM1_H
|
||||
#define PRIPWM1_H
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <limits.h>
|
||||
#include <unistd.h>
|
||||
#include <sys/mman.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/stat.h>
|
||||
#include <fcntl.h>
|
||||
/***********************************************************************
|
||||
* Author: Hussam al-Hertani (Hertaville.com)
|
||||
* Others are free to modify and use this code as they see fit so long as they
|
||||
* give credit to the author.
|
||||
*
|
||||
* The author is not liable for any kind of damage caused by this software.
|
||||
*
|
||||
* Acknowledgements: This 'C++' class is based on 'C' code available from :
|
||||
* - code from http://elinux.org/RPi_Low-level_peripherals
|
||||
* - http://www.raspberrypi.org/phpBB3/viewtopic.php?t=8467&p=124620 for PWM initialization
|
||||
* - frank's code...http://www.frank-buss.de/raspberrypi/pwm.c
|
||||
* - Gertboard's C source code
|
||||
*
|
||||
* The rpiPWM1 class provides a direct memory mapped (register-based)
|
||||
* interface to the PWM1 hardware on the Raspberry Pi's BCM2835 SOC.
|
||||
* The BCM2835 SOC was two PWM subsystems, PWM1 & PWM2. This code will
|
||||
* enable access to the PWM1 subsystem which outputs the PWM signal on
|
||||
* GPIO18 (ALT5).
|
||||
*
|
||||
* The class enables setting the Frequency (Max 19.2MHz), PWM resolution(4095),
|
||||
* DutyCycle and PWM Mode to be used. The Duty Cycle can be set as a
|
||||
* percentage (setDutyCycle() or setDutyCycleForce()) or as a function
|
||||
* of the PWM Resoultion (setDutyCycleCount())
|
||||
*
|
||||
* Two PWM modes exist:
|
||||
* - MSMODE - This is the traditional PWM Mode i.e. if PWM Resolution
|
||||
* (counts) is 1024 and the dutyCycle (Pulse on time) is 512 (in counts or 50%)
|
||||
* then the waveform would look like:
|
||||
* |||||||||||||||||_________________
|
||||
* MSMODE is ideal for servos and other applications that
|
||||
* require classical PWM waveforms
|
||||
* - PWMMODE - Is a slightly modified version of the traditional PWM Mode
|
||||
* described above. The duty cycle or ON time is still unchanged
|
||||
* within the period but is distributed across the entire period instead
|
||||
* on being concentrated in the first part of the period..i.e.if PWM Resolution
|
||||
* (counts) is 1024 and the dutyCycle (Pulse on time) is 512 (in counts or 50%)
|
||||
* then the waveform would look like:
|
||||
* |_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_
|
||||
* This mode is ideal if you want to pass the signal through
|
||||
* a low pass filter to obtain an analog signal equivalent to the . The even
|
||||
* distibution of the ON time through the entire period
|
||||
* significantly reduces ripple caused by using a simple RC
|
||||
* low pass filter
|
||||
*
|
||||
* When setting the frequency via the constructor or 'setFrequency' method, one is strictly
|
||||
* requesting a particular frequency. The code will do its best to get as close as possible to
|
||||
* the requested frequency but it is likely to not create a PWM at the exact requested Frequency.
|
||||
* As an example say that we want to create a PWM waveform with a resolution of 256 counts (8-bit)
|
||||
* and a frequency of 8KHz....We get the divisor using the following algorithm
|
||||
*
|
||||
* Waveform period = 1 / 8KHz = 0.125ms
|
||||
* Duration of a single count = period/256 = 0.125ms / 256 = 0.488us
|
||||
* Frequency of a single count = 1 / Duration of a single count = 1 / 0.488us = 2.048MHz
|
||||
* Divisor value = floor (PWM Clock Frequency / Frequency of a single count) = floor (19.2MHz / 2.048MHz) = floor(9.375) = 9
|
||||
*
|
||||
* With a Divisor of 9 Actual Waveform Frequency = 1/((1/(19.2MHz/9))*256) = 8.333 KHz
|
||||
*
|
||||
* The actual Frequency will generally deviate further from the desired frequency as the count value (PWM resolution)
|
||||
* increases i.e. As an example say that we want to create a PWM waveform with a resolution of 1024 counts (10-bit)
|
||||
* and the same frequency as the above example:
|
||||
*
|
||||
* Waveform period = 1 / 8KHz = 0.125ms
|
||||
* Duration of a single count = period/1024 = 0.125ms / 1024 = 122.070ns
|
||||
* Frequency of a single count = 1 / Duration of a single count = 1 / 122.070ns = 8.192MHz
|
||||
* Divisor value = floor (PWM Clock Frequency / Frequency of a single count)
|
||||
* = floor (19.2MHz / 8.192MHz) = floor(2.34) = 2
|
||||
*
|
||||
* With a Divisor of 2, Actual Waveform Frequency = 1/((1/(19.2MHz/2))*1024) = 9.375KHz
|
||||
*
|
||||
* DIVISOR MUST BE AT LEAST 2....SO PICK YOUR COUNT AND DESIRED FREQUENCY VALUES CAREFULLY!!!!!
|
||||
* i.e MAXIMUM FREQUENCY FOR 10-BIT RESOLUTION (COUNT=1024) IS 9.375KHz
|
||||
* & MAXIMUM FREQUENCY FOR 8-BIT RESOLUTION (COUNT=256) IS 37.5KHz
|
||||
*
|
||||
* WARNING: The RPI uses the PWM1 subsystem to produce audio. As such
|
||||
* please refrain from playing audio on the RPI while this code
|
||||
* is running.
|
||||
* *********************************************************************/
|
||||
class rpiPWM1 {
|
||||
|
||||
public:
|
||||
rpiPWM1();
|
||||
// default constructor configures GPIO18 for PWM and Frequency 1000Hz,
|
||||
// PWM resolution (counts) 256, Duty Cycle 50% and PWM mode is 'PWMMODE'
|
||||
rpiPWM1(double Hz, unsigned int cnts, double duty, int m);
|
||||
//overloaded constructor..allows user to set initial values for Frequency,
|
||||
//PWM resolution, Duty Cycle and PWM mode.
|
||||
~rpiPWM1();
|
||||
// Destructor....safely releases all mapped memory and puts all used peripherals
|
||||
// (PWM clock, PWM peripheral and GPIO peripheral in their initial states
|
||||
unsigned int setFrequency(const double &hz);
|
||||
// Sets Frequency and reinitializes PWM peripheral
|
||||
unsigned int setCounts(const unsigned int &cnts);
|
||||
// Sets PWM resolution (counts) and reinitializes PWM peripheral
|
||||
unsigned int setDutyCycle(const double &duty);
|
||||
// Sets Duty Cycle as a Percentage (Fast)
|
||||
unsigned int setDutyCycleCount(const unsigned int &cnts );
|
||||
// Sets Duty Cycle as a count value (Fast) i.e. if counts is 1024
|
||||
// and 'duty' is set to 512, a 50% duty cycle is achieved
|
||||
unsigned int setDutyCycleForce(const double &duty, const int &m);
|
||||
// disables PWM peripheral first,
|
||||
//Sets Duty Cycle as a Percentage and PWM mode...
|
||||
// then enables PWM peripheral
|
||||
unsigned int setMode(const int &m);
|
||||
// sets PWM mode...calls 'setDutyCycleForce()'
|
||||
|
||||
|
||||
double getFrequency() const;
|
||||
// returns current Frequency of PWM waveform
|
||||
double getDutyCycle() const;
|
||||
// returns current DutyCycle (as a %) of PWM waveform
|
||||
int getCounts() const;
|
||||
// returns PWM resolution
|
||||
int getDivisor() const;
|
||||
//returns Divisor value used to set the period per count
|
||||
//as a function of the default PWM clock Frequency of 19.2MHz
|
||||
int getMode() const;
|
||||
//returns (1) if current PWM mode is 'PWMMODE' or (2) if current PWM mode
|
||||
//is 'MSMODE'
|
||||
|
||||
//Public constants
|
||||
static const int PWMMODE = 1;
|
||||
static const int MSMODE = 2;
|
||||
//Two PWM modes
|
||||
static const int ERRFREQ = 1;
|
||||
static const int ERRCOUNT = 2;
|
||||
static const int ERRDUTY = 3;
|
||||
static const int ERRMODE = 4;
|
||||
//Error Codes
|
||||
|
||||
private:
|
||||
//Private constants
|
||||
static const int BCM2708_PERI_BASE = 0x20000000;
|
||||
static const int PWM_BASE = (BCM2708_PERI_BASE + 0x20C000); /* PWM controller */
|
||||
static const int CLOCK_BASE = (BCM2708_PERI_BASE + 0x101000); /* Clock controller */
|
||||
static const int GPIO_BASE = (BCM2708_PERI_BASE + 0x200000); /* GPIO controller */
|
||||
//Base register addresses
|
||||
static const int PWM_CTL = 0;
|
||||
static const int PWM_RNG1 = 4;
|
||||
static const int PWM_DAT1 = 5;
|
||||
static const int PWMCLK_CNTL= 40;
|
||||
static const int PWMCLK_DIV = 41;
|
||||
// Register addresses offsets divided by 4 (register addresses are word (32-bit) aligned
|
||||
static const int BLOCK_SIZE = 4096;
|
||||
// Block size.....every time mmap() is called a 4KB
|
||||
//section of real physical memory is mapped into the memory of
|
||||
//the process
|
||||
|
||||
|
||||
volatile unsigned *mapRegAddr(unsigned long baseAddr);
|
||||
// this function is used to map physical memory
|
||||
void configPWM1Pin();
|
||||
//this function sets GPIO18 to the alternat function 5 (ALT5)
|
||||
// to enable the pin to output the PWM waveforms generated by PWM1
|
||||
void configPWM1();
|
||||
//This function is responsible for the global configuration and initialixation
|
||||
//of the the PWM1 peripheral
|
||||
|
||||
double frequency; // PWM frequency
|
||||
double dutyCycle; //PWM duty Cycle (%)
|
||||
unsigned int counts; // PWM resolution
|
||||
unsigned int divisor; // divisor value
|
||||
int mode; // PWM mode
|
||||
volatile unsigned *clk, *pwm, *gpio; // pointers to the memory mapped sections
|
||||
//of our process memory
|
||||
|
||||
};
|
||||
#endif
|
||||
|
|
@ -0,0 +1,44 @@
|
|||
#include "rpiServo.h"
|
||||
|
||||
const int rpiServo::ERRDEG;// Error code
|
||||
|
||||
/*********************************************************************************
|
||||
* rpiServo constructor - Calls rpiPWM1's overloaded constructor
|
||||
* which basically creates a 50Hz PWM waveform, wih a resolution count of 3600,
|
||||
* a duty cycle of 7.5% (1.5ms ON pulse every 20ms to center servo shaft at
|
||||
* 90 degree position) and traditional PWM mode (rpiPWM1::MSMODE)
|
||||
*
|
||||
* By setting count resolution to 3600 counts, one 20ms period is equivalent to
|
||||
* to 3600 counts, 2ms ON time (180 degree position) is equivalent to 360 counts
|
||||
* and 1ms ON time (0 degree position) is equivalent to 180 counts. This gives us
|
||||
* 180 counts between the 0 degree position and the 180 degree position hence we
|
||||
* get 1 degree rotation resolution
|
||||
*
|
||||
*********************************************************************************/
|
||||
|
||||
rpiServo::rpiServo():rpiPWM1(50.0,3600,7.5,rpiPWM1::MSMODE){
|
||||
// 20ms = 3600 counts (Period)
|
||||
// 2ms = 360 counts (10% duty cycle) => angle 180
|
||||
// 1ms = 180 counts (5% duty cycle) => angle 0
|
||||
// 1.5ms = 180+90 = 270 (7.5% duty cycle ) => angle = 90 //servo centered
|
||||
}
|
||||
|
||||
|
||||
/************************************************************************
|
||||
*setAngle() - function sets the angle of the servo's shaft
|
||||
*Parameters - degrees - new shaft angle. Can be any value between 0 and 180 only
|
||||
*
|
||||
*return Value - If parameter 'degrees' was between 0 and 180 (inclusive) then
|
||||
return value is 0 else its spiServo::ERRDEG
|
||||
***********************************************************************/
|
||||
|
||||
unsigned int rpiServo::setAngle(unsigned int degrees){
|
||||
unsigned int retVal = 0;
|
||||
|
||||
if((degrees < 0) || (degrees > 180 ))
|
||||
retVal = ERRDEG;
|
||||
else
|
||||
this->setDutyCycleCount(180 + degrees); // call the necessary rpiPWM1 method
|
||||
|
||||
return retVal;
|
||||
}
|
||||
|
|
@ -0,0 +1,26 @@
|
|||
#ifndef RPI_SERVO_H
|
||||
#define RPI_SERVO_H
|
||||
|
||||
#include "rpiPWM1.h"
|
||||
|
||||
/******************************************************************
|
||||
* rpiServo - This tiny C++ class is able to generate waveforms necessary to
|
||||
* control servos. This class is derived from the rpiPWM1 class.
|
||||
* It consists of a constructor that creates a 50Hz PWM waveform with
|
||||
* a 1.5ms ON time pulse...causing the servo to center itself to the
|
||||
* 90 degree position.
|
||||
*
|
||||
* The class also consists on a setAngle method that sets the angle of
|
||||
* rotation to anything between 0 & 180 degrees
|
||||
* ****************************************************************/
|
||||
|
||||
class rpiServo : public rpiPWM1
|
||||
{
|
||||
|
||||
public:
|
||||
rpiServo();
|
||||
unsigned int setAngle(unsigned int degrees);
|
||||
static const int ERRDEG=1;
|
||||
};
|
||||
|
||||
#endif
|
||||
Loading…
Reference in New Issue