main.cpp

00001 #include "mbed.h"
00002 #include "WiiNunchuck.h"
00003 #include "Servo.h"
00004 
00005 #define LED1 PC_10
00006 #define LED2 PC_12
00007 #define LED3 PC_11
00008 
00009 #define SERVO1 PA_6
00010 #define SERVO2 PA_7
00011 #define SERVO3 PB_11
00012 #define SERVO4 PB_1
00013 #define SERVO5 PB_10
00014 #define SERVO6 PB_3
00015 
00016 //#define PWM_PERIOD 20 // ms
00017 //#define PULSE_MIN 500.0 // us
00018 //#define PULSE_MAX 2500.0 // us
00019 
00020 
00021 #define SWITCH1 PC_8
00022 #define SWITCH2 PC_6
00023 
00024 #define SDA PC_1
00025 #define SCL PC_0
00026 
00027 #define POW_EN PA_4
00028 
00029 WiiNunchuck nchk(SDA, SCL);
00030 Serial pc(USBTX, USBRX);
00031 
00032 DigitalOut led1(LED1);
00033 DigitalOut led2(LED2);
00034 DigitalOut led3(LED3);
00035 DigitalInOut pow_en(POW_EN);
00036 
00037 InterruptIn switch1(SWITCH1);
00038 InterruptIn switch2(SWITCH2);
00039 
00040 Servo servo_vert(SERVO1);
00041 Servo servo_horz(SERVO2);
00042 Servo servo_rot(SERVO4);
00043 Servo servo_grab(SERVO5);
00044 
00045 int accx, accy, accz;
00046 unsigned char joyx, joyy;
00047 bool buttonx, buttonz;
00048 
00049 void init();
00050 void switch1_irq();
00051 void switch2_irq();
00052 void enable_servos(bool on);
00053 void servo_angle(PwmOut *servo, float angle);
00054 
00055 bool servos_enabled = false;
00056 
00057 
00058 // main() runs in its own thread in the OS
00059 int main()
00060 {
00061     init();
00062     
00063     while(1)
00064     {
00065         char key = pc.getc();
00066         if(key==0x1B)
00067         {                               //If the keypress was an arrow key
00068             key = pc.getc();                        //Check again!
00069             if (key == 0x5B)
00070             {
00071                 key = pc.getc();
00072                 switch(key) {                         //Check to see which arrow key...
00073                     case 0x41:                        //It was the UP arrow key...
00074                         servo_vert = servo_vert + 0.01;
00075                         pc.printf("vert %f", servo_vert.read());
00076                         break;
00077                     case 0x42:                        //It was the DOWN arrow key...
00078                         servo_vert = servo_vert - 0.01;
00079                         pc.printf("vert %f", servo_vert.read());
00080                         break;
00081                     case 0x43:                        //It was the RIGHT arrow key...
00082                         servo_rot = servo_rot + 0.01;
00083                         pc.printf("\n\r RIGHT!");
00084                         break;
00085                     case 0x44:                        //It was the LEFT arrow key...
00086                         servo_rot = servo_rot - 0.01;
00087                         pc.printf("\n\r LEFT!");
00088                         break;
00089                 }
00090             }
00091         }
00092         
00093         wait(0.01);  
00094     }
00095 }
00096 
00097 void init()
00098 {
00099     pow_en.output();
00100     pow_en.mode(OpenDrain);
00101     enable_servos(false);
00102     
00103     servo_vert.calibrate(0.0007, 90)
00104 
00105     servo_vert = 0.5;
00106     servo_horz = 0.5;
00107     servo_rot = 0.5;
00108     servo_grab = 0.5;
00109     /*servo_horz.position(90);
00110     servo_rot.position(90);
00111     servo_grab.position(90);*/
00112     
00113     switch1.mode(PullUp);
00114     switch1.fall(&switch1_irq);
00115     switch2.mode(PullUp);
00116     switch2.fall(&switch2_irq);
00117     
00118     //led1 = 1;
00119 }
00120 
00121 void switch1_irq()
00122 {
00123     if(servos_enabled)
00124         enable_servos(false);
00125     else
00126         enable_servos(true);
00127 }
00128 
00129 void switch2_irq()
00130 {
00131 }
00132 
00133 void enable_servos(bool on)
00134 {
00135     if(on)
00136     {
00137         pow_en.output();
00138         pow_en = 0;
00139         servos_enabled = true;
00140         led2 = 1; 
00141     }
00142     else
00143     {
00144         pow_en = 1;
00145         //pow_en.mode(OpenDrain);
00146         pow_en.input();
00147         servos_enabled = false;
00148         led2 = 0; 
00149     }
00150 }
00151