Naoyuki SAITO
/
moter_titech_ns
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
moter_titech
by 
y w
main.cpp
- Committer:
- nekoyyy
- Date:
- 2013-12-14
- Revision:
- 7:1947b554f8e3
- Parent:
- 3:c92e21f305d8
- Child:
- 8:3eb58ae94a93
File content as of revision 7:1947b554f8e3:
#include "mbed.h"
#include "rtos.h"
Serial pc(USBTX, USBRX);
PwmOut Kata(p21);
PwmOut Hiji(p22);
PwmOut Te(p23);
PwmOut Dou(p24);
PwmOut Asi(p25);
char c;
int zen=0;
int go=0;
int dousa=0;
int men=0;
int bou=0;
int tuk=0;
int dou=0;
int fei=1;
float homeK = 0.028f; //home pos 0.045-0.105
float homeH = 0.103f;
float homeT = 0.045f;
float henni(float a, float b){
float temp = a;
temp = b - a;
return (temp);
}
float maxof(float a, float b, float c){
float A = abs(a);
float B = abs(b);
float C = abs(c);
float max = A;
if (B >= max) max = B;
if (C >= max) max = C;
return (max);
}
void HANTEI(void const *argument) {
while(true){
switch(c){
default:
c=pc.getc();
break;
case'z':
zen=1;
c=0;
pc.printf("zen\n");
break;
case'n':
zen=0;
go=0;
c=0;
pc.printf("stop\n");
break;
case'g':
go=1;
c=0;
pc.printf("go\n");
break;
case'm':
if(dousa==0){
men=1;
dousa=1;
pc.printf("men\n");
}
c=0;
break;
case'b':
if(dousa==0){
bou=1;
dousa=1;
pc.printf("bou\n");
}
c=0;
break;
case't':
if(dousa==0){
tuk=1;
dousa=1;
pc.printf("tuk\n");
}
c=0;
break;
case'd':
if(dousa==0){
dou=1;
dousa=1;
pc.printf("dou\n");
}
c=0;
break;
case'f':
if((bou==0)&&(fei==1)&&(dousa==1)){
fei=2;
pc.printf("fei\n");
}
c=0;
break;
/*pc.printf("men=%d",men);
pc.printf("bou=%d",bou);
pc.printf("tuk=%d",tuk);
pc.printf("dou=%d",dou);
pc.printf("fei=%d\n",fei);
wait(0.5);*/
}
}
}
void ZENGO(void const *argument) {
Asi.period(0.00001);
while(true){
if(zen==1){
Asi.pulsewidth(0.00000f);
}
else if(go==1){
Asi.pulsewidth(0.00001f);
}
else{
Asi.pulsewidth(0.000005f);
}
}
}
void MEN(void const *argument) {
int i1=0;
float a1=0.055f; //sireiti
float b1=0.101f;
float c1=0.045f;
float max1 = maxof(henni(homeK,a1),henni(homeH,b1),henni(homeT,c1));
float t1 = max1 / (0.03f / 0.17f * 0.01f); //nannbyou kakeruka 60(0.03duty)/0.17 servo /0.02s
int i2=0;
float a2=0.028f; //sireiti
float b2=0.111f;
float c2=0.045f;
float max2 = maxof(henni(a1,a2),henni(b1,b2),henni(c1,c2));
float t2 = max2 / (0.03f / 0.17f * 0.01f); //nannbyou kakeruka 60(0.03duty)/0.17 servo /0.02s
while(true){
if(men==1){
switch(fei){
case 1: //men motion
i1=i1+1;
Kata = homeK + (a1 - homeK) / t1 * i1;
Hiji = homeH + (b1 - homeH) / t1 * i1;
Te = homeT + (c1 - homeT) / t1 * i1;
wait(0.022);
if(i1>t1){
fei=3;
}
break;
case 2: //feint
i1=i1-1;
Kata = homeK + (a1 - homeK) / t1 * i1;
Hiji = homeH + (b1 - homeH) / t1 * i1;
Te = homeT + (c1 - homeT) / t1 * i1;
wait(0.022);
if(i1<0){
fei=1;
men=0;
dousa=0;
i1=1;
}
break;
case 3: //men iki
i2=i2+1;
Kata = a1 + (a2 - a1) / t2 * i2;
Hiji = b1 + (b2 - b1) / t2 * i2;
Te = c1 + (c2 - c1) / t2 * i2;
wait(0.022);
if(i2>t2){
fei=4;
}
break;
case 4: //men kaeri
i2=i2-1;
Kata = a1 + (a2 - a1) / t2 * i2;
Hiji = b1 + (b2 - b1) / t2 * i2;
Te = c1+ (c2 - c1) / t2 * i2;
wait(0.022);
if(i2<0){
i2=1;
fei=5;
}
break;
case 5: //home kaeri
i1=i1-1;
Kata = homeK + (a1 - homeK) / t1 * i1;
Hiji = homeH + (b1 - homeH) / t1 * i1;
Te = homeT + (c1 - homeT) / t1 * i1;
wait(0.022);
if(i1<0){
i1=1;
fei=1;
dousa=0;
men=0;
}
break;
}
}
}
}
void BOUGYO(void const *argument) {
int i=0;
float a=0.075f; //sireiti
float b=0.100f;
float c=0.035f;
float max = maxof(henni(homeK,a),henni(homeH,b),henni(homeT,c));
float t = max / (0.03f / 0.17f * 0.01f); //nannbyou kakeruka 60(0.03duty)/0.17 servo /0.02s
while(true){
if(bou==1){
switch(fei){
case 1: //men iki
i=i+1;
Kata = homeK + (a - homeK) / t * i;
Hiji = homeH + (b - homeH) / t * i;
Te = homeT + (c - homeT) / t * i;
wait(0.022);
if(i>t){
fei=3;
}
break;
case 3: //men kaeri
i=i-1;
Kata = homeK + (a - homeK) / t * i;
Hiji = homeH + (b - homeH) / t * i;
Te = homeT + (c - homeT) / t * i;
wait(0.022);
if(i<0){
fei=1;
bou=0;
dousa=0;
i=1;
}
break;
}
}
}
}
void TUKI(void const *argument) {
int i=0;
float a=0.056f; //sireiti
float b=0.084f;
float c=0.035f;
float max = maxof(henni(homeK,a),henni(homeH,b),henni(homeT,c));
float t = max / (0.03f / 0.17f * 0.01f); //nannbyou kakeruka 60(0.03duty)/0.17 servo /0.02s
while(true){
if(tuk==1){
switch(fei){
case 1: //men iki
i=i+1;
Kata = homeK + (a - homeK) / t * i;
Hiji = homeH + (b - homeH) / t * i;
Te = homeT + (c - homeT) / t * i;
wait(0.022);
if(i>t){
fei=3;
}
break;
case 2: //feint
i=i-1;
Kata = homeK + (a - homeK) / t * i;
Hiji = homeH + (b - homeH) / t * i;
Te = homeT + (c - homeT) / t * i;
wait(0.022);
if(i<0){
fei=1;
tuk=0;
dousa=0;
i=1;
}
break;
case 3: //men kaeri
i=i-1;
Kata = homeK + (a - homeK) / t * i;
Hiji = homeH + (b - homeH) / t * i;
Te = homeT + (c - homeT) / t * i;
wait(0.022);
if(i<0){
fei=1;
tuk=0;
dousa=0;
i=1;
}
break;
}
}
}
}
void DOU(void const *argument) {
int i=0;
float a=0.047f; //sireiti
float b=0.087f;
float c=0.045f;
float max = maxof(henni(homeK,a),henni(homeH,b),henni(homeT,c));
float t = max / (0.03f / 0.17f * 0.01f); //nannbyou kakeruka 60(0.03duty)/0.17 servo /0.02s
while(true){
if(dou==1){
switch(fei){
case 1: //men iki
i=i+1;
Kata = homeK + (a - homeK) / t * i;
Hiji = homeH + (b - homeH) / t * i;
Te = homeT + (c - homeT) / t * i;
wait(0.022);
if(i>t){
fei=3;
}
break;
case 2: //feint
i=i-1;
Kata = homeK + (a - homeK) / t * i;
Hiji = homeH + (b - homeH) / t * i;
Te = homeT + (c - homeT) / t * i;
wait(0.022);
if(i<0){
fei=1;
dou=0;
dousa=0;
i=1;
}
break;
case 3: //men kaeri
i=i-1;
Kata = homeK + (a - homeK) / t * i;
Hiji = homeH + (b - homeH) / t * i;
Te = homeT + (c - homeT) / t * i;
wait(0.022);
if(i<0){
fei=1;
dou=0;
dousa=0;
i=1;
}
break;
}
}
}
}
int main() {
Thread t1(HANTEI);
Thread t2(ZENGO);
Thread t3(MEN);
Thread t4(BOUGYO);
Thread t5(TUKI);
Thread t6(DOU);
Kata=homeK;
Hiji=homeH;
Te=homeT;
pc.printf("startfm\n");
while (1){
}
}