PS3 version
Dependencies: Motor_NIT_Nagaoka_College PID QEI SoftServo SoftPWM mbed DebounceIn
Fork of NHK2015 by
main.cpp@3:d2c733b52600, 2015-09-02 (annotated)
- Committer:
- WAT34
- Date:
- Wed Sep 02 00:19:34 2015 +0000
- Revision:
- 3:d2c733b52600
- Parent:
- 2:74c543a0a671
- Child:
- 4:646562d80dc2
??????????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
WAT34 | 3:d2c733b52600 | 1 | #define pi 3.141593 |
WAT34 | 0:00fcc71314cf | 2 | #include "mbed.h" |
WAT34 | 0:00fcc71314cf | 3 | #include "Motor.h" |
WAT34 | 3:d2c733b52600 | 4 | #include "PID.h" |
WAT34 | 3:d2c733b52600 | 5 | #include "QEI.h" |
WAT34 | 3:d2c733b52600 | 6 | #define RATE 0.01 |
WAT34 | 3:d2c733b52600 | 7 | BusOut air(p13,p14); |
WAT34 | 3:d2c733b52600 | 8 | DigitalOut out(p12); |
WAT34 | 3:d2c733b52600 | 9 | Serial conn(p9,p10); |
WAT34 | 2:74c543a0a671 | 10 | Serial pc(USBTX,USBRX); |
WAT34 | 3:d2c733b52600 | 11 | Motor L(p22,p15,p16); |
WAT34 | 3:d2c733b52600 | 12 | Motor R(p23,p17,p18); |
WAT34 | 3:d2c733b52600 | 13 | PID Lp(1.0,0,0,RATE); |
WAT34 | 3:d2c733b52600 | 14 | PID Rp(1.0,0,0,RATE); |
WAT34 | 3:d2c733b52600 | 15 | PID Tp(1.0,0,0,RATE); |
WAT34 | 3:d2c733b52600 | 16 | BusOut led(LED1,LED2,LED3,LED4); |
WAT34 | 3:d2c733b52600 | 17 | Motor ot(p21,p19,p20); |
WAT34 | 2:74c543a0a671 | 18 | QEI sensort(p29,p30,NC,624); |
WAT34 | 3:d2c733b52600 | 19 | QEI ls(p27,p28,NC,624); |
WAT34 | 3:d2c733b52600 | 20 | QEI rs(p25,p26,NC,624); |
WAT34 | 3:d2c733b52600 | 21 | Timeout ai; |
WAT34 | 3:d2c733b52600 | 22 | Ticker getsp; |
WAT34 | 0:00fcc71314cf | 23 | char read; |
WAT34 | 3:d2c733b52600 | 24 | int Rs = 0,Ls = 0; |
WAT34 | 3:d2c733b52600 | 25 | int i = 0; |
WAT34 | 3:d2c733b52600 | 26 | void getspeed() |
WAT34 | 3:d2c733b52600 | 27 | { |
WAT34 | 3:d2c733b52600 | 28 | Rs =rs.getPulses(); |
WAT34 | 3:d2c733b52600 | 29 | Ls =ls.getPulses(); |
WAT34 | 3:d2c733b52600 | 30 | rs.reset(); |
WAT34 | 3:d2c733b52600 | 31 | ls.reset(); |
WAT34 | 3:d2c733b52600 | 32 | } |
WAT34 | 3:d2c733b52600 | 33 | void zero(){ |
WAT34 | 3:d2c733b52600 | 34 | air = 0; |
WAT34 | 3:d2c733b52600 | 35 | i = 0; |
WAT34 | 3:d2c733b52600 | 36 | out = 1; |
WAT34 | 3:d2c733b52600 | 37 | } |
WAT34 | 3:d2c733b52600 | 38 | void rev(){ |
WAT34 | 3:d2c733b52600 | 39 | air = 2; |
WAT34 | 3:d2c733b52600 | 40 | ai.attach(&zero,1.0); |
WAT34 | 3:d2c733b52600 | 41 | out = 0; |
WAT34 | 3:d2c733b52600 | 42 | } |
WAT34 | 2:74c543a0a671 | 43 | int main() |
WAT34 | 0:00fcc71314cf | 44 | { |
WAT34 | 3:d2c733b52600 | 45 | double tilt = 0; |
WAT34 | 3:d2c733b52600 | 46 | int8_t ttilt = 0; |
WAT34 | 3:d2c733b52600 | 47 | Lp.setInputLimits(-3000,3000); |
WAT34 | 3:d2c733b52600 | 48 | Rp.setInputLimits(-3000,3000); |
WAT34 | 3:d2c733b52600 | 49 | Tp.setInputLimits(-10000,10000); |
WAT34 | 3:d2c733b52600 | 50 | Lp.setOutputLimits(-0.9,0.9); |
WAT34 | 3:d2c733b52600 | 51 | Rp.setOutputLimits(-0.9,0.9); |
WAT34 | 3:d2c733b52600 | 52 | Tp.setOutputLimits(-0.9,0.9); |
WAT34 | 3:d2c733b52600 | 53 | Lp.setMode(1); |
WAT34 | 3:d2c733b52600 | 54 | Rp.setMode(1); |
WAT34 | 3:d2c733b52600 | 55 | Tp.setMode(1); |
WAT34 | 3:d2c733b52600 | 56 | Lp.setBias(0); |
WAT34 | 3:d2c733b52600 | 57 | Rp.setBias(0); |
WAT34 | 3:d2c733b52600 | 58 | Tp.setBias(0); |
WAT34 | 3:d2c733b52600 | 59 | getsp.attach(&getspeed,0.1); |
WAT34 | 0:00fcc71314cf | 60 | while(1) { |
WAT34 | 3:d2c733b52600 | 61 | if(conn.getc() == 255) { |
WAT34 | 0:00fcc71314cf | 62 | read = conn.getc(); |
WAT34 | 3:d2c733b52600 | 63 | ttilt = conn.getc(); |
WAT34 | 0:00fcc71314cf | 64 | } |
WAT34 | 3:d2c733b52600 | 65 | if((read>>2)%2 && i == 0){ |
WAT34 | 3:d2c733b52600 | 66 | air = 1; |
WAT34 | 3:d2c733b52600 | 67 | ai.attach(&rev,1.0); |
WAT34 | 3:d2c733b52600 | 68 | i = 1; |
WAT34 | 3:d2c733b52600 | 69 | led = 1; |
WAT34 | 0:00fcc71314cf | 70 | } |
WAT34 | 3:d2c733b52600 | 71 | if((read>>3)%2){ |
WAT34 | 3:d2c733b52600 | 72 | Lp.setSetPoint(100); |
WAT34 | 3:d2c733b52600 | 73 | Rp.setSetPoint(100); |
WAT34 | 3:d2c733b52600 | 74 | }else |
WAT34 | 3:d2c733b52600 | 75 | if((read>>4)%2){ |
WAT34 | 3:d2c733b52600 | 76 | Lp.setSetPoint(-100); |
WAT34 | 3:d2c733b52600 | 77 | Rp.setSetPoint(-100); |
WAT34 | 3:d2c733b52600 | 78 | led = 2; |
WAT34 | 3:d2c733b52600 | 79 | }else |
WAT34 | 3:d2c733b52600 | 80 | if((read>>5)%2){ |
WAT34 | 3:d2c733b52600 | 81 | Lp.setSetPoint(-100); |
WAT34 | 3:d2c733b52600 | 82 | Rp.setSetPoint(100); |
WAT34 | 3:d2c733b52600 | 83 | led = 4; |
WAT34 | 3:d2c733b52600 | 84 | }else |
WAT34 | 3:d2c733b52600 | 85 | if((read>>6)%2){ |
WAT34 | 3:d2c733b52600 | 86 | Lp.setSetPoint(100); |
WAT34 | 3:d2c733b52600 | 87 | Rp.setSetPoint(-100); |
WAT34 | 3:d2c733b52600 | 88 | led = 8; |
WAT34 | 3:d2c733b52600 | 89 | }else{ |
WAT34 | 3:d2c733b52600 | 90 | Lp.setSetPoint(0); |
WAT34 | 3:d2c733b52600 | 91 | Rp.setSetPoint(0); |
WAT34 | 3:d2c733b52600 | 92 | led = 0; |
WAT34 | 0:00fcc71314cf | 93 | } |
WAT34 | 3:d2c733b52600 | 94 | Tp.setSetPoint(ttilt); |
WAT34 | 3:d2c733b52600 | 95 | tilt = sensort.getPulses(); |
WAT34 | 3:d2c733b52600 | 96 | //tilt = tilt*24.0/500.0*pi/360.0; |
WAT34 | 3:d2c733b52600 | 97 | conn.putc(int8_t(tilt)); |
WAT34 | 3:d2c733b52600 | 98 | Lp.setProcessValue(Ls); |
WAT34 | 3:d2c733b52600 | 99 | Rp.setProcessValue(Rs); |
WAT34 | 3:d2c733b52600 | 100 | Tp.setProcessValue(tilt); |
WAT34 | 3:d2c733b52600 | 101 | L.speed(Lp.compute()); |
WAT34 | 3:d2c733b52600 | 102 | R.speed(Rp.compute()); |
WAT34 | 3:d2c733b52600 | 103 | ot.speed(Tp.compute()); |
WAT34 | 3:d2c733b52600 | 104 | pc.printf("%f\n\r",Lp.compute()); |
WAT34 | 0:00fcc71314cf | 105 | } |
WAT34 | 0:00fcc71314cf | 106 | } |