divaldo
/
PiCar
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Working version

This commit is contained in:
David Baranyai 2018-06-06 21:58:54 +02:00
commit 6727b4fdc9
9 changed files with 1046 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
.dep.inc Normal file
View File

@ -0,0 +1,5 @@
# This code depends on make tool being used
DEPFILES=$(wildcard $(addsuffix .d, ${OBJECTFILES} ${TESTOBJECTFILES}))
ifneq (${DEPFILES},)
include ${DEPFILES}
endif

17
Makefile Normal file
View File

@ -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

83
joystick.cc Normal file
View File

@ -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;
}

154
joystick.hh Normal file
View File

@ -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

109
main.cpp Normal file
View File

@ -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;
}

431
rpiPWM1.cpp Normal file
View File

@ -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) );
}

177
rpiPWM1.h Normal file
View File

@ -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

44
rpiServo.cpp Normal file
View File

@ -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;
}

26
rpiServo.h Normal file
View File

@ -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