bio robot
Dependencies: MPU6050-DMP QEI_hw mbed-rpc mbed
Fork of MPU6050_Example by
main.cpp@19:270735e44c98, 2015-12-11 (annotated)
- Committer:
- amandaghassaei
- Date:
- Fri Dec 11 08:53:08 2015 +0000
- Revision:
- 19:270735e44c98
- Parent:
- 18:0cfe72d8a006
obstacle debugging
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
amandaghassaei | 3:be5cf450436d | 1 | #include "mbed.h" |
amandaghassaei | 7:7efcd3bf3302 | 2 | #include "mbed_rpc.h" |
amandaghassaei | 2:17b18ea93551 | 3 | #include "Comm.h" |
amandaghassaei | 7:7efcd3bf3302 | 4 | #include "Controls.h" |
amandaghassaei | 3:be5cf450436d | 5 | |
amandaghassaei | 7:7efcd3bf3302 | 6 | Controls controls; |
amandaghassaei | 9:1d9b24d7ac77 | 7 | Comm comm(&controls); |
amandaghassaei | 9:1d9b24d7ac77 | 8 | |
amandaghassaei | 9:1d9b24d7ac77 | 9 | long serialCounter = 0; |
amandaghassaei | 9:1d9b24d7ac77 | 10 | |
amandaghassaei | 9:1d9b24d7ac77 | 11 | |
amandaghassaei | 10:769cc457c3a4 | 12 | Ticker controlsInterrupt; |
amandaghassaei | 10:769cc457c3a4 | 13 | |
syundo0730 | 0:8d2c753a96e7 | 14 | |
amandaghassaei | 6:12b6be801ad8 | 15 | int main() { |
amandaghassaei | 9:1d9b24d7ac77 | 16 | controls.setPC(comm.getPC()); |
amandaghassaei | 10:769cc457c3a4 | 17 | controls.setup(); |
amandaghassaei | 9:1d9b24d7ac77 | 18 | // comm.printPosition(); |
amandaghassaei | 11:711d3c207e8c | 19 | comm.printGains(); |
amandaghassaei | 10:769cc457c3a4 | 20 | |
amandaghassaei | 10:769cc457c3a4 | 21 | controlsInterrupt.attach_us(&controls, &Controls::loop, 1000); |
amandaghassaei | 10:769cc457c3a4 | 22 | |
amandaghassaei | 6:12b6be801ad8 | 23 | while(1) { |
amandaghassaei | 10:769cc457c3a4 | 24 | controls.updateIMUS(); |
amandaghassaei | 6:12b6be801ad8 | 25 | comm.check(); |
amandaghassaei | 11:711d3c207e8c | 26 | |
amandaghassaei | 18:0cfe72d8a006 | 27 | |
amandaghassaei | 11:711d3c207e8c | 28 | if (serialCounter++>100) { |
amandaghassaei | 11:711d3c207e8c | 29 | // comm.printPosition(); |
amandaghassaei | 10:769cc457c3a4 | 30 | // comm.getPC()->printf("%f\n", controls.getTheta1()); |
amandaghassaei | 10:769cc457c3a4 | 31 | // comm.getPC()->printf("%f", controls.motor.getPWM()); |
amandaghassaei | 9:1d9b24d7ac77 | 32 | serialCounter = 0; |
amandaghassaei | 12:49813131dd15 | 33 | // float z[4] = {1,2,0,0}; |
amandaghassaei | 12:49813131dd15 | 34 | // comm.getPC()->printf("%f\n",controls.target.getTheta2ForTarget(z)); |
amandaghassaei | 9:1d9b24d7ac77 | 35 | } |
amandaghassaei | 6:12b6be801ad8 | 36 | } |
amandaghassaei | 6:12b6be801ad8 | 37 | } |
amandaghassaei | 6:12b6be801ad8 | 38 | |
amandaghassaei | 6:12b6be801ad8 | 39 | |
amandaghassaei | 10:769cc457c3a4 | 40 | |
amandaghassaei | 10:769cc457c3a4 | 41 | |
amandaghassaei | 10:769cc457c3a4 | 42 | |
amandaghassaei | 6:12b6be801ad8 | 43 | //wrappers for comm stack |
amandaghassaei | 5:c3d671959ef3 | 44 | void openGripper1Wrapper(Arguments * input, Reply * output){ |
amandaghassaei | 5:c3d671959ef3 | 45 | comm.openGripper1(input, output); |
amandaghassaei | 5:c3d671959ef3 | 46 | }; |
amandaghassaei | 5:c3d671959ef3 | 47 | RPCFunction OpenGripper1(&openGripper1Wrapper, "OpenGripper1"); |
amandaghassaei | 5:c3d671959ef3 | 48 | void closeGripper1Wrapper(Arguments * input, Reply * output){ |
amandaghassaei | 5:c3d671959ef3 | 49 | comm.closeGripper1(input, output); |
amandaghassaei | 5:c3d671959ef3 | 50 | }; |
amandaghassaei | 5:c3d671959ef3 | 51 | RPCFunction CloseGripper1(&closeGripper1Wrapper, "CloseGripper1"); |
amandaghassaei | 17:8a0e647cf551 | 52 | |
amandaghassaei | 17:8a0e647cf551 | 53 | |
amandaghassaei | 5:c3d671959ef3 | 54 | void openGripper2Wrapper(Arguments * input, Reply * output){ |
amandaghassaei | 5:c3d671959ef3 | 55 | comm.openGripper2(input, output); |
amandaghassaei | 5:c3d671959ef3 | 56 | }; |
amandaghassaei | 5:c3d671959ef3 | 57 | RPCFunction OpenGripper2(&openGripper2Wrapper, "OpenGripper2"); |
amandaghassaei | 5:c3d671959ef3 | 58 | void closeGripper2Wrapper(Arguments * input, Reply * output){ |
amandaghassaei | 5:c3d671959ef3 | 59 | comm.closeGripper2(input, output); |
amandaghassaei | 5:c3d671959ef3 | 60 | }; |
amandaghassaei | 5:c3d671959ef3 | 61 | RPCFunction CloseGripper2(&closeGripper2Wrapper, "CloseGripper2"); |
amandaghassaei | 17:8a0e647cf551 | 62 | |
amandaghassaei | 17:8a0e647cf551 | 63 | |
amandaghassaei | 9:1d9b24d7ac77 | 64 | void setSwingUpKWrapper(Arguments * input, Reply * output){ |
amandaghassaei | 9:1d9b24d7ac77 | 65 | comm.setSwingUpK(input, output); |
amandaghassaei | 5:c3d671959ef3 | 66 | }; |
amandaghassaei | 9:1d9b24d7ac77 | 67 | RPCFunction SetSwingUpK(&setSwingUpKWrapper, "SetSwingUpK"); |
amandaghassaei | 9:1d9b24d7ac77 | 68 | void setSwingUpDWrapper(Arguments * input, Reply * output){ |
amandaghassaei | 9:1d9b24d7ac77 | 69 | comm.setSwingUpD(input, output); |
amandaghassaei | 9:1d9b24d7ac77 | 70 | }; |
amandaghassaei | 9:1d9b24d7ac77 | 71 | RPCFunction SetSwingUpD(&setSwingUpDWrapper, "SetSwingUpD"); |
amandaghassaei | 17:8a0e647cf551 | 72 | |
amandaghassaei | 17:8a0e647cf551 | 73 | |
amandaghassaei | 14:d620415259b1 | 74 | void setDesiredThetaPWrapper(Arguments * input, Reply * output){ |
amandaghassaei | 14:d620415259b1 | 75 | comm.setDesiredThetaP(input, output); |
amandaghassaei | 10:769cc457c3a4 | 76 | }; |
amandaghassaei | 14:d620415259b1 | 77 | RPCFunction SetDesiredThetaP(&setDesiredThetaPWrapper, "SetDesiredThetaP"); |
amandaghassaei | 19:270735e44c98 | 78 | void setSoftLimitsPWrapper(Arguments * input, Reply * output){ |
amandaghassaei | 19:270735e44c98 | 79 | comm.setSoftLimitsP(input, output); |
amandaghassaei | 19:270735e44c98 | 80 | }; |
amandaghassaei | 19:270735e44c98 | 81 | RPCFunction SetSoftLimitsP(&setSoftLimitsPWrapper, "SetSoftLimitsP"); |
amandaghassaei | 17:8a0e647cf551 | 82 | |
amandaghassaei | 17:8a0e647cf551 | 83 | |
amandaghassaei | 9:1d9b24d7ac77 | 84 | void setTargetWrapper(Arguments * input, Reply * output){ |
amandaghassaei | 5:c3d671959ef3 | 85 | comm.setTarget(input, output); |
amandaghassaei | 5:c3d671959ef3 | 86 | }; |
amandaghassaei | 9:1d9b24d7ac77 | 87 | RPCFunction SetTarget(&setTargetWrapper, "SetTarget"); |
amandaghassaei | 17:8a0e647cf551 | 88 | |
amandaghassaei | 17:8a0e647cf551 | 89 | void setTargetingKWrapper(Arguments * input, Reply * output){ |
amandaghassaei | 17:8a0e647cf551 | 90 | comm.setTargetingK(input, output); |
amandaghassaei | 17:8a0e647cf551 | 91 | }; |
amandaghassaei | 17:8a0e647cf551 | 92 | RPCFunction SetTargetingK(&setTargetingKWrapper, "SetTargetingK"); |
amandaghassaei | 17:8a0e647cf551 | 93 | void setTargetingDWrapper(Arguments * input, Reply * output){ |
amandaghassaei | 17:8a0e647cf551 | 94 | comm.setTargetingD(input, output); |
amandaghassaei | 17:8a0e647cf551 | 95 | }; |
amandaghassaei | 17:8a0e647cf551 | 96 | RPCFunction SetTargetingD(&setTargetingDWrapper, "SetTargetingD"); |
amandaghassaei | 17:8a0e647cf551 | 97 | |
amandaghassaei | 17:8a0e647cf551 | 98 | |
amandaghassaei | 17:8a0e647cf551 | 99 | |
amandaghassaei | 17:8a0e647cf551 | 100 | |
amandaghassaei | 11:711d3c207e8c | 101 | void setThetaWrapper(Arguments * input, Reply * output){ |
amandaghassaei | 11:711d3c207e8c | 102 | comm.setTheta(input, output); |
amandaghassaei | 9:1d9b24d7ac77 | 103 | }; |
amandaghassaei | 11:711d3c207e8c | 104 | RPCFunction SetTheta(&setThetaWrapper, "SetTheta"); |
amandaghassaei | 17:8a0e647cf551 | 105 | |
amandaghassaei | 17:8a0e647cf551 | 106 | |
amandaghassaei | 10:769cc457c3a4 | 107 | void printGainsWrapper(Arguments * input, Reply * output){ |
amandaghassaei | 10:769cc457c3a4 | 108 | comm.printGains(); |
amandaghassaei | 10:769cc457c3a4 | 109 | }; |
amandaghassaei | 10:769cc457c3a4 | 110 | RPCFunction PrintGains(&printGainsWrapper, "PrintGains"); |
amandaghassaei | 10:769cc457c3a4 | 111 | void printPositionWrapper(Arguments * input, Reply * output){ |
amandaghassaei | 10:769cc457c3a4 | 112 | comm.printPosition(); |
amandaghassaei | 10:769cc457c3a4 | 113 | }; |
amandaghassaei | 10:769cc457c3a4 | 114 | RPCFunction PrintPosition(&printPositionWrapper, "PrintPosition"); |
amandaghassaei | 5:c3d671959ef3 | 115 | |
amandaghassaei | 5:c3d671959ef3 | 116 | |
amandaghassaei | 5:c3d671959ef3 | 117 | |
amandaghassaei | 5:c3d671959ef3 | 118 | |
syundo0730 | 0:8d2c753a96e7 | 119 | |
amandaghassaei | 10:769cc457c3a4 | 120 |