-
Notifications
You must be signed in to change notification settings - Fork 0
/
pwm.c
68 lines (62 loc) · 2.03 KB
/
pwm.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
#include <assert.h>
#include <stdlib.h>
#include <fcntl.h>
#include <sys/mman.h>
#include "pwm.h" //camsi.ups-tlse.fr/lib/exe/fetch.php?media=embedded:rm-mpu-6000a-00v4.2.pdf
#include "common.h"
/////////// GLOBAL ////////////
#define PWM1 ctx.pwm1
#define PWM2 ctx.pwm2
#define GPIO ctx.gpio
#ifndef WCET
void*mapGet(int start){
int devmem_fd = open("/dev/mem", O_RDWR|O_SYNC);
assert(devmem_fd>=0);
void*map_addr=mmap(NULL,getpagesize(),PROT_READ|PROT_WRITE,MAP_SHARED,devmem_fd,start);
close(devmem_fd);
assert(map_addr!=MAP_FAILED);
return map_addr;
}
///////////// P W M //////////
void pwmInit(){
ctx.pwm1=mapGet(PWM_1_START);
ctx.pwm2=mapGet(PWM_2_START);
ctx.gpio=mapGet(GPIO_1_START);
PWM1->cmpa=PWM1->cmpb=PWM2->cmpa=PWM2->cmpb=SPEED_0;
}
void pwmStop(){
PWM1->cmpa=PWM1->cmpb=PWM2->cmpa=PWM2->cmpb=SPEED_0;
munmap(&ctx.pwm1,getpagesize());
munmap(&ctx.pwm2,getpagesize());
munmap(&ctx.gpio,getpagesize());
}
#endif
#define MINMAX(val) (val>SPEED_MAX?SPEED_MAX:(val<SPEED_MIN)?SPEED_MIN:val)
void setSpeed(int motor, int throttle, int r_computed, int p_computed){
//valeur comprise entre 3600 et 6000
if(motor & MOTOR_FL) PWM2->cmpa = MINMAX(throttle + p_computed - r_computed);
if(motor & MOTOR_FR) PWM1->cmpa = MINMAX(throttle + p_computed + r_computed);
if(motor & MOTOR_BL) PWM2->cmpb = MINMAX(throttle - p_computed - r_computed);
if(motor & MOTOR_BR) PWM1->cmpb = MINMAX(throttle - p_computed + r_computed);
}
/*
int main(){
setvbuf(stdout, NULL, _IONBF, 0);//debufferise stdout (pour les \r via putty)
mpuInit();
pwmInit();
#define H 4800
#define M 3000
#define L 3000
#define S .1
PWM1->cmpa=PWM1->cmpb=PWM2->cmpa=PWM2->cmpb=L;
while(1){
MPU_Data g=mpuGet();
printf("% 4.2f % 4.2f % 4.2f % 4.2f % 4.2f % 4.2f \r",g.AccX,g.AccY,g.AccZ,g.GyrX,g.GyrY,g.GyrZ);
PWM1->cmpa=((g.AccX>+S) || (g.AccY>+S))?H:M;//NE++
PWM1->cmpb=((g.AccX<-S) || (g.AccY>+S))?H:M;//SE-+
PWM2->cmpa=((g.AccX>+S) || (g.AccY<-S))?H:M;//NW+-
PWM2->cmpb=((g.AccX<-S) || (g.AccY<-S))?H:M;//SW--
}
pwmStop();
return 0;
}*/