PS3 version
Dependencies: Motor_NIT_Nagaoka_College PID QEI SoftServo SoftPWM mbed DebounceIn
Fork of NHK2015 by
main.cpp@10:fefff533c442, 2015-09-16 (annotated)
- Committer:
- WAT34
- Date:
- Wed Sep 16 08:33:58 2015 +0000
- Revision:
- 10:fefff533c442
- Parent:
- 8:9d8999740604
????
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 | 8:9d8999740604 | 6 | #include "Servo.h" |
WAT34 | 4:646562d80dc2 | 7 | #define RATE 0.05 |
WAT34 | 10:fefff533c442 | 8 | DigitalOut led(LED1); |
WAT34 | 8:9d8999740604 | 9 | BusOut air(PC_10,PC_11); |
WAT34 | 8:9d8999740604 | 10 | DigitalOut out(PC_12); |
WAT34 | 10:fefff533c442 | 11 | Serial conn(NC,PA_12); |
WAT34 | 10:fefff533c442 | 12 | Serial pc(SERIAL_TX, SERIAL_RX); |
WAT34 | 4:646562d80dc2 | 13 | PID Tp(50,40000,0,0.001); |
WAT34 | 8:9d8999740604 | 14 | Motor ot(PB_13,PB_3,PA_10); |
WAT34 | 8:9d8999740604 | 15 | QEI sensort(PC_3,PC_0,NC,624); |
WAT34 | 8:9d8999740604 | 16 | Servo L(PC_9); |
WAT34 | 8:9d8999740604 | 17 | Servo R(PC_8); |
WAT34 | 3:d2c733b52600 | 18 | Timeout ai; |
WAT34 | 0:00fcc71314cf | 19 | char read; |
WAT34 | 3:d2c733b52600 | 20 | int Rs = 0,Ls = 0; |
WAT34 | 3:d2c733b52600 | 21 | int i = 0; |
WAT34 | 3:d2c733b52600 | 22 | void zero(){ |
WAT34 | 3:d2c733b52600 | 23 | air = 0; |
WAT34 | 3:d2c733b52600 | 24 | i = 0; |
WAT34 | 3:d2c733b52600 | 25 | out = 1; |
WAT34 | 3:d2c733b52600 | 26 | } |
WAT34 | 3:d2c733b52600 | 27 | void rev(){ |
WAT34 | 3:d2c733b52600 | 28 | air = 2; |
WAT34 | 3:d2c733b52600 | 29 | ai.attach(&zero,1.0); |
WAT34 | 3:d2c733b52600 | 30 | out = 0; |
WAT34 | 3:d2c733b52600 | 31 | } |
WAT34 | 2:74c543a0a671 | 32 | int main() |
WAT34 | 0:00fcc71314cf | 33 | { |
WAT34 | 10:fefff533c442 | 34 | pc.printf("connected\n\r"); |
WAT34 | 10:fefff533c442 | 35 | //conn.baud(115200); |
WAT34 | 10:fefff533c442 | 36 | //ot.setfrequency(60000); |
WAT34 | 4:646562d80dc2 | 37 | double tilt = 0,lo = 0,ro = 0; |
WAT34 | 4:646562d80dc2 | 38 | int8_t ttilt = 0,tmpread = 0,tmpttilt = 0; |
WAT34 | 5:4b462b9cb255 | 39 | char tro = 0,tlo = 0; |
WAT34 | 4:646562d80dc2 | 40 | Tp.setInputLimits(-45,45); |
WAT34 | 3:d2c733b52600 | 41 | Tp.setOutputLimits(-0.9,0.9); |
WAT34 | 3:d2c733b52600 | 42 | Tp.setMode(1); |
WAT34 | 10:fefff533c442 | 43 | led=1; |
WAT34 | 4:646562d80dc2 | 44 | Tp.setBias(0.0); |
WAT34 | 0:00fcc71314cf | 45 | while(1) { |
WAT34 | 10:fefff533c442 | 46 | if(conn.getc() == 0xFF) { |
WAT34 | 10:fefff533c442 | 47 | led = !led; |
WAT34 | 4:646562d80dc2 | 48 | tmpread = conn.getc(); |
WAT34 | 4:646562d80dc2 | 49 | tmpttilt = conn.getc(); |
WAT34 | 5:4b462b9cb255 | 50 | tro = conn.getc(); |
WAT34 | 5:4b462b9cb255 | 51 | tlo = conn.getc(); |
WAT34 | 5:4b462b9cb255 | 52 | if(tmpread^tmpttilt^tro^tlo == conn.getc()){ |
WAT34 | 4:646562d80dc2 | 53 | ttilt = tmpttilt; |
WAT34 | 4:646562d80dc2 | 54 | read = tmpread; |
WAT34 | 10:fefff533c442 | 55 | ro = tro; |
WAT34 | 10:fefff533c442 | 56 | lo = tlo; |
WAT34 | 4:646562d80dc2 | 57 | } |
WAT34 | 0:00fcc71314cf | 58 | } |
WAT34 | 3:d2c733b52600 | 59 | if((read>>2)%2 && i == 0){ |
WAT34 | 3:d2c733b52600 | 60 | air = 1; |
WAT34 | 3:d2c733b52600 | 61 | ai.attach(&rev,1.0); |
WAT34 | 3:d2c733b52600 | 62 | i = 1; |
WAT34 | 0:00fcc71314cf | 63 | } |
WAT34 | 3:d2c733b52600 | 64 | Tp.setSetPoint(ttilt); |
WAT34 | 4:646562d80dc2 | 65 | tilt = double(sensort.getPulses()); |
WAT34 | 4:646562d80dc2 | 66 | tilt = tilt*61/5128.0; |
WAT34 | 3:d2c733b52600 | 67 | Tp.setProcessValue(tilt); |
WAT34 | 4:646562d80dc2 | 68 | if (abs(lo) < 0.1){ |
WAT34 | 4:646562d80dc2 | 69 | lo = 0; |
WAT34 | 4:646562d80dc2 | 70 | } |
WAT34 | 4:646562d80dc2 | 71 | if (abs(ro) < 0.1){ |
WAT34 | 4:646562d80dc2 | 72 | ro = 0; |
WAT34 | 4:646562d80dc2 | 73 | } |
WAT34 | 10:fefff533c442 | 74 | L= (lo-127)/127.0; |
WAT34 | 10:fefff533c442 | 75 | R=(ro-127)/127.0; |
WAT34 | 3:d2c733b52600 | 76 | ot.speed(Tp.compute()); |
WAT34 | 7:920cbfb28112 | 77 | //pc.printf("%f %d\n\r",Tp.compute(),read); |
WAT34 | 10:fefff533c442 | 78 | pc.printf("%d-%f\r\n",read,lo); |
WAT34 | 0:00fcc71314cf | 79 | } |
WAT34 | 0:00fcc71314cf | 80 | } |