bio robot

Dependencies:   MPU6050-DMP QEI_hw mbed-rpc mbed

Fork of MPU6050_Example by Shundo Kishi

Committer:
amandaghassaei
Date:
Fri Dec 11 08:54:32 2015 +0000
Revision:
20:f13b949b623b
Parent:
19:270735e44c98
publish

Who changed what in which revision?

UserRevisionLine numberNew 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