main.cpp

00001 #include "mbed.h"
00002 #include "mbed_rpc.h" 
00003 #include "Comm.h"
00004 #include "Controls.h"
00005 
00006 Controls controls;
00007 Comm comm(&controls);
00008 
00009 long serialCounter = 0;
00010 
00011 
00012 Ticker controlsInterrupt;
00013 
00014 
00015 int main() {
00016     controls.setPC(comm.getPC());
00017     controls.setup();
00018 //    comm.printPosition();
00019     comm.printGains();
00020 
00021     controlsInterrupt.attach_us(&controls, &Controls::loop, 1000);
00022 
00023     while(1) {
00024         controls.updateIMUS();
00025         comm.check();
00026         
00027         
00028         if (serialCounter++>100) {
00029 //            comm.printPosition();
00030 //            comm.getPC()->printf("%f\n", controls.getTheta1());
00031 //            comm.getPC()->printf("%f", controls.motor.getPWM());
00032             serialCounter = 0;
00033            // float z[4] = {1,2,0,0};
00034 //            comm.getPC()->printf("%f\n",controls.target.getTheta2ForTarget(z));
00035         }
00036     }
00037 }
00038 
00039 
00040 
00041 
00042 
00043 //wrappers for comm stack
00044 void openGripper1Wrapper(Arguments * input, Reply * output){
00045     comm.openGripper1(input, output);
00046 };
00047 RPCFunction OpenGripper1(&openGripper1Wrapper, "OpenGripper1");
00048 void closeGripper1Wrapper(Arguments * input, Reply * output){
00049     comm.closeGripper1(input, output);
00050 };
00051 RPCFunction CloseGripper1(&closeGripper1Wrapper, "CloseGripper1");
00052 
00053 
00054 void openGripper2Wrapper(Arguments * input, Reply * output){
00055     comm.openGripper2(input, output);
00056 };
00057 RPCFunction OpenGripper2(&openGripper2Wrapper, "OpenGripper2");
00058 void closeGripper2Wrapper(Arguments * input, Reply * output){
00059     comm.closeGripper2(input, output);
00060 };
00061 RPCFunction CloseGripper2(&closeGripper2Wrapper, "CloseGripper2");
00062 
00063 
00064 void setSwingUpKWrapper(Arguments * input, Reply * output){
00065     comm.setSwingUpK(input, output);
00066 };
00067 RPCFunction SetSwingUpK(&setSwingUpKWrapper, "SetSwingUpK");
00068 void setSwingUpDWrapper(Arguments * input, Reply * output){
00069     comm.setSwingUpD(input, output);
00070 };
00071 RPCFunction SetSwingUpD(&setSwingUpDWrapper, "SetSwingUpD");
00072 
00073 
00074 void setDesiredThetaPWrapper(Arguments * input, Reply * output){
00075     comm.setDesiredThetaP(input, output);
00076 };
00077 RPCFunction SetDesiredThetaP(&setDesiredThetaPWrapper, "SetDesiredThetaP");
00078 void setSoftLimitsPWrapper(Arguments * input, Reply * output){
00079     comm.setSoftLimitsP(input, output);
00080 };
00081 RPCFunction SetSoftLimitsP(&setSoftLimitsPWrapper, "SetSoftLimitsP");
00082 
00083 
00084 void setTargetWrapper(Arguments * input, Reply * output){
00085     comm.setTarget(input, output);
00086 };
00087 RPCFunction SetTarget(&setTargetWrapper, "SetTarget");
00088 
00089 void setTargetingKWrapper(Arguments * input, Reply * output){
00090     comm.setTargetingK(input, output);
00091 };
00092 RPCFunction SetTargetingK(&setTargetingKWrapper, "SetTargetingK");
00093 void setTargetingDWrapper(Arguments * input, Reply * output){
00094     comm.setTargetingD(input, output);
00095 };
00096 RPCFunction SetTargetingD(&setTargetingDWrapper, "SetTargetingD");
00097 
00098 
00099 
00100 
00101 void setThetaWrapper(Arguments * input, Reply * output){
00102     comm.setTheta(input, output);
00103 };
00104 RPCFunction SetTheta(&setThetaWrapper, "SetTheta");
00105 
00106 
00107 void printGainsWrapper(Arguments * input, Reply * output){
00108     comm.printGains();
00109 };
00110 RPCFunction PrintGains(&printGainsWrapper, "PrintGains");
00111 void printPositionWrapper(Arguments * input, Reply * output){
00112     comm.printPosition();
00113 };
00114 RPCFunction PrintPosition(&printPositionWrapper, "PrintPosition");
00115 
00116 
00117 
00118 
00119 
00120