廣智 角谷
/
LineTracer
ライントレーサー制御用のプログラムです.
motor.cpp@0:e9af471e2a2b, 2017-11-16 (annotated)
- Committer:
- Hirotomo777
- Date:
- Thu Nov 16 06:15:58 2017 +0000
- Revision:
- 0:e9af471e2a2b
?7????????????????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Hirotomo777 | 0:e9af471e2a2b | 1 | |
Hirotomo777 | 0:e9af471e2a2b | 2 | #include "motor.h" |
Hirotomo777 | 0:e9af471e2a2b | 3 | BusOut leftc(PTA1,PTA2); |
Hirotomo777 | 0:e9af471e2a2b | 4 | PwmOut leftp(PTD4); |
Hirotomo777 | 0:e9af471e2a2b | 5 | |
Hirotomo777 | 0:e9af471e2a2b | 6 | BusOut rightc(PTC0,PTC7); |
Hirotomo777 | 0:e9af471e2a2b | 7 | PwmOut rightp(PTA12); |
Hirotomo777 | 0:e9af471e2a2b | 8 | |
Hirotomo777 | 0:e9af471e2a2b | 9 | PwmOut handle(PTC9); |
Hirotomo777 | 0:e9af471e2a2b | 10 | |
Hirotomo777 | 0:e9af471e2a2b | 11 | //start code for servo |
Hirotomo777 | 0:e9af471e2a2b | 12 | void servoInit(){ |
Hirotomo777 | 0:e9af471e2a2b | 13 | handle.period(0.020); |
Hirotomo777 | 0:e9af471e2a2b | 14 | handle.pulsewidth(0.00155); |
Hirotomo777 | 0:e9af471e2a2b | 15 | wait(0.5); |
Hirotomo777 | 0:e9af471e2a2b | 16 | } |
Hirotomo777 | 0:e9af471e2a2b | 17 | |
Hirotomo777 | 0:e9af471e2a2b | 18 | |
Hirotomo777 | 0:e9af471e2a2b | 19 | void rotate(PwmOut servo,float ppulse,float *npulse){ |
Hirotomo777 | 0:e9af471e2a2b | 20 | if(ppulse > *npulse){ |
Hirotomo777 | 0:e9af471e2a2b | 21 | if(ppulse > 0.002){ |
Hirotomo777 | 0:e9af471e2a2b | 22 | ppulse = 0.002; |
Hirotomo777 | 0:e9af471e2a2b | 23 | } |
Hirotomo777 | 0:e9af471e2a2b | 24 | servo.pulsewidth(ppulse); // servo position determined by a pulsewidth between 1-2ms |
Hirotomo777 | 0:e9af471e2a2b | 25 | wait(1000.0 * 0.06 * ((ppulse - (*npulse)) / 0.1)); |
Hirotomo777 | 0:e9af471e2a2b | 26 | } |
Hirotomo777 | 0:e9af471e2a2b | 27 | else { |
Hirotomo777 | 0:e9af471e2a2b | 28 | if(ppulse < 0.0011){ |
Hirotomo777 | 0:e9af471e2a2b | 29 | ppulse = 0.0011; |
Hirotomo777 | 0:e9af471e2a2b | 30 | } |
Hirotomo777 | 0:e9af471e2a2b | 31 | servo.pulsewidth(ppulse); // servo position determined by a pulsewidth between 1-2ms |
Hirotomo777 | 0:e9af471e2a2b | 32 | wait(1000.0 * 0.06 * (((*npulse) - ppulse) / 0.1)); |
Hirotomo777 | 0:e9af471e2a2b | 33 | } |
Hirotomo777 | 0:e9af471e2a2b | 34 | *npulse = ppulse; |
Hirotomo777 | 0:e9af471e2a2b | 35 | } |
Hirotomo777 | 0:e9af471e2a2b | 36 | |
Hirotomo777 | 0:e9af471e2a2b | 37 | /* |
Hirotomo777 | 0:e9af471e2a2b | 38 | float rotate(PwmOut servo,float ppulse){ |
Hirotomo777 | 0:e9af471e2a2b | 39 | static float npulse = 0.00155; |
Hirotomo777 | 0:e9af471e2a2b | 40 | if(ppulse > npulse){ |
Hirotomo777 | 0:e9af471e2a2b | 41 | if(ppulse > 0.002){ |
Hirotomo777 | 0:e9af471e2a2b | 42 | ppulse = 0.002; |
Hirotomo777 | 0:e9af471e2a2b | 43 | } |
Hirotomo777 | 0:e9af471e2a2b | 44 | servo.pulsewidth(ppulse); // servo position determined by a pulsewidth between 1-2ms |
Hirotomo777 | 0:e9af471e2a2b | 45 | wait(1000.0 * 0.06 * ((ppulse - (npulse)) / 0.1)); |
Hirotomo777 | 0:e9af471e2a2b | 46 | } |
Hirotomo777 | 0:e9af471e2a2b | 47 | else { |
Hirotomo777 | 0:e9af471e2a2b | 48 | if(ppulse < 0.0011){ |
Hirotomo777 | 0:e9af471e2a2b | 49 | ppulse = 0.0011; |
Hirotomo777 | 0:e9af471e2a2b | 50 | } |
Hirotomo777 | 0:e9af471e2a2b | 51 | servo.pulsewidth(ppulse); // servo position determined by a pulsewidth between 1-2ms |
Hirotomo777 | 0:e9af471e2a2b | 52 | wait(1000.0 * 0.06 * (((npulse) - ppulse) / 0.1)); |
Hirotomo777 | 0:e9af471e2a2b | 53 | } |
Hirotomo777 | 0:e9af471e2a2b | 54 | npulse = ppulse; |
Hirotomo777 | 0:e9af471e2a2b | 55 | return npulse; |
Hirotomo777 | 0:e9af471e2a2b | 56 | } |
Hirotomo777 | 0:e9af471e2a2b | 57 | */ |
Hirotomo777 | 0:e9af471e2a2b | 58 | //end |
Hirotomo777 | 0:e9af471e2a2b | 59 | |
Hirotomo777 | 0:e9af471e2a2b | 60 | //start code for DCmotor |
Hirotomo777 | 0:e9af471e2a2b | 61 | void motorInit(){ |
Hirotomo777 | 0:e9af471e2a2b | 62 | leftc = 0; |
Hirotomo777 | 0:e9af471e2a2b | 63 | leftp = 1.0f; |
Hirotomo777 | 0:e9af471e2a2b | 64 | rightc = 0; |
Hirotomo777 | 0:e9af471e2a2b | 65 | rightp = 1.0f; |
Hirotomo777 | 0:e9af471e2a2b | 66 | } |
Hirotomo777 | 0:e9af471e2a2b | 67 | |
Hirotomo777 | 0:e9af471e2a2b | 68 | void start(){ |
Hirotomo777 | 0:e9af471e2a2b | 69 | leftc = 1; |
Hirotomo777 | 0:e9af471e2a2b | 70 | rightc = 1; |
Hirotomo777 | 0:e9af471e2a2b | 71 | } |
Hirotomo777 | 0:e9af471e2a2b | 72 | |
Hirotomo777 | 0:e9af471e2a2b | 73 | void stop(){ |
Hirotomo777 | 0:e9af471e2a2b | 74 | leftc = 0; |
Hirotomo777 | 0:e9af471e2a2b | 75 | rightc = 0; |
Hirotomo777 | 0:e9af471e2a2b | 76 | } |
Hirotomo777 | 0:e9af471e2a2b | 77 | //end |