main.cpp

00001 /*
00002 * Servo motor program for OBON  by Tomoki Hokida
00003 *
00004 * Use RTOS in this program
00005 *
00006 * RTOSをバリバリ使用しているのであしあらず
00007 * サーボを４つ使うことになったらSERVO_4をdefineしましょう
00008 */
00009 
00010 #include "mbed.h"
00011 #include "Servo.h"
00012 #include "rtos.h"
00013 
00014 #define DEFAULT_R 50
00015 #define DEFAULT_L 50
00016 #define DEFAULT_U 50
00017 #define DEFAULT_D 50
00018 
00019 #define SERVO_4
00020 
00021 DigitalOut led1(LED1);
00022 DigitalOut led2(LED2);
00023 
00024 #ifdef SERVO_4
00025 DigitalOut led3(LED3);
00026 DigitalOut led4(LED4);
00027 #endif
00028 
00029 DigitalIn  sw[4] = {p17, p19, p18, p20};
00030 
00031 //DigitalIn swU(p17);
00032 //DigitalIn swD(p18);
00033 //DigitalIn swR(p19);
00034 //DigitalIn swL(p20);
00035 //Servo obonU(p21);
00036 //Servo obonR(p22);
00037 //Servo obonD(p23);
00038 //Servo obonL(p24);
00039 
00040 
00041 #ifdef SERVO_4
00042 
00043 Servo obon[4] = { p21, p22, p23, p24};
00044 
00045 #else
00046 
00047 Servo obon[2] = {p21,p22};
00048 
00049 #endif
00050 
00051 void obonThreadR(void const *argument){
00052 
00053     for(int j=DEFAULT_R;;){
00054 
00055         obon[1] = j/100.0;
00056         
00057          if((!sw[1])&&(j<100.0)){j++;}
00058          if((!sw[3])&&(j>0)){j--;}
00059          
00060          led1 = !led1;
00061          
00062         Thread::wait(10);
00063     }
00064 }
00065 
00066 void obonThreadU(void const *argument){
00067     for(int k=DEFAULT_U; ;){
00068         
00069         obon[0] = k/100.0;
00070         
00071         if((!sw[0])&&(k<100.0)){k++;}
00072         if((!sw[2])&&(k>0)){k--;}
00073         
00074         led2 = !led2;
00075         
00076         Thread::wait(10);
00077     }
00078 }
00079 
00080 #ifdef SERVO_4
00081 
00082 void obonThreadL(void const *argument){
00083     for(int i=DEFAULT_L;;){
00084         
00085         obon[3] = i/100.0;
00086         
00087         if((!sw[3])&&(i<100.0)){i++;}
00088         if((!sw[1])&&(i>0)){i--;}
00089         
00090         led3 = !led3;
00091         
00092         Thread::wait(10);
00093     }
00094 }
00095 
00096 void obonThreadD(void const *argument){
00097     for(int l=DEFAULT_D;;){
00098         
00099         obon[2] = l/100.0;
00100         
00101         if((!sw[2])&&(l<=100.0)){l++;}
00102         if((!sw[1])&&(l>=0)){l--;}
00103         
00104         led4 = !led4;
00105         
00106         Thread::wait(10);
00107     }
00108 }
00109 #endif
00110 
00111 int main() {
00112     
00113     sw[0].mode(PullUp);
00114     sw[1].mode(PullUp);
00115     sw[2].mode(PullUp);
00116     sw[3].mode(PullUp);
00117  
00118     
00119     Thread thread1(obonThreadU);
00120     Thread thread2(obonThreadR);
00121     
00122 #ifdef SERVO_4
00123         
00124     Thread thread3(obonThreadD);
00125     Thread thread4(obonThreadL);
00126 
00127 #endif
00128     for(;;);
00129 
00130 }