涼太郎 中村 / Mbed 2 deprecated f3rc2

めいん

Dependencies:   mbed

main.cpp@5:1c95260515c1, 2016-09-03 (annotated)

Committer:
choutin
Date:
Sat Sep 03 06:51:48 2016 +0000
Revision:
5:1c95260515c1
Parent:
4:dcb03da10fa7 
test for you

Who changed what in which revision?

UserRevisionLine numberNew contents of line
choutin 5:1c95260515c1 1 #include "mbed.h"
choutin 5:1c95260515c1 2
choutin 5:1c95260515c1 3 PwmOut pwmarm(PC_6);
choutin 5:1c95260515c1 4 PwmOut pwmhand(PC_8);
choutin 5:1c95260515c1 5 DigitalOut led(LED1);
choutin 5:1c95260515c1 6 PwmOut M1cw(PA_11);
choutin 5:1c95260515c1 7 PwmOut M1ccw(PB_15);
choutin 5:1c95260515c1 8 PwmOut M2ccw(PB_14);
choutin 5:1c95260515c1 9 PwmOut M2cw(PB_13);
choutin 5:1c95260515c1 10 float PERIOD=20000;
choutin 5:1c95260515c1 11
choutin 5:1c95260515c1 12 void initmotor()
choutin 5:1c95260515c1 13 {
choutin 5:1c95260515c1 14
choutin 5:1c95260515c1 15
choutin 5:1c95260515c1 16 M1cw.period_us(256);
choutin 5:1c95260515c1 17 M1ccw.period_us(256);
choutin 5:1c95260515c1 18 M2cw.period_us(256);
choutin 5:1c95260515c1 19 M2ccw.period_us(256);
choutin 5:1c95260515c1 20
choutin 5:1c95260515c1 21 }
choutin 5:1c95260515c1 22
choutin 5:1c95260515c1 23 void move(int left,int right)
choutin 5:1c95260515c1 24 {
choutin 5:1c95260515c1 25
choutin 5:1c95260515c1 26 float rightduty,leftduty;
choutin 0:df2659fd8031 27
choutin 5:1c95260515c1 28 if(right>256) {
choutin 5:1c95260515c1 29 right=256;
choutin 5:1c95260515c1 30 }
choutin 5:1c95260515c1 31 if(left>256) {
choutin 5:1c95260515c1 32 left=256;
choutin 5:1c95260515c1 33 }
choutin 5:1c95260515c1 34 if(right<-256) {
choutin 5:1c95260515c1 35 right=-256;
choutin 5:1c95260515c1 36 }
choutin 5:1c95260515c1 37 if(left<-256) {
choutin 5:1c95260515c1 38 left=-256;
choutin 5:1c95260515c1 39 }
choutin 5:1c95260515c1 40
choutin 5:1c95260515c1 41 rightduty=right/256.0;
choutin 5:1c95260515c1 42 leftduty=left/256.0;
choutin 5:1c95260515c1 43 if(right>0) {
choutin 5:1c95260515c1 44 M1cw.write(1-rightduty);
choutin 5:1c95260515c1 45 M1ccw.write(1);
choutin 5:1c95260515c1 46 } else {
choutin 5:1c95260515c1 47 M1cw.write(1);
choutin 5:1c95260515c1 48 M1ccw.write(1+rightduty);
choutin 5:1c95260515c1 49 }
choutin 5:1c95260515c1 50
choutin 5:1c95260515c1 51 if(left>0) {
choutin 5:1c95260515c1 52 M2cw.write(1-leftduty);
choutin 5:1c95260515c1 53 M2ccw.write(1);
choutin 5:1c95260515c1 54 } else {
choutin 5:1c95260515c1 55 M2cw.write(1);
choutin 5:1c95260515c1 56 M2ccw.write(1+leftduty);
choutin 5:1c95260515c1 57 }
choutin 2:b204cf2f9b60 58 }
choutin 0:df2659fd8031 59
choutin 0:df2659fd8031 60
choutin 5:1c95260515c1 61 void close_hand(void){
choutin 5:1c95260515c1 62 int i,degree;
choutin 5:1c95260515c1 63
choutin 5:1c95260515c1 64 pwmhand.period_ms(20); //20ms
choutin 5:1c95260515c1 65
choutin 5:1c95260515c1 66 degree=175;
choutin 2:b204cf2f9b60 67
choutin 5:1c95260515c1 68 i=500+degree*1900/180;
choutin 5:1c95260515c1 69 pwmhand.write(i/PERIOD);
choutin 5:1c95260515c1 70
choutin 5:1c95260515c1 71
choutin 5:1c95260515c1 72 }
choutin 5:1c95260515c1 73
choutin 5:1c95260515c1 74 void close_arm(void) {
choutin 5:1c95260515c1 75 int i,degree;
choutin 5:1c95260515c1 76
choutin 5:1c95260515c1 77 pwmarm.period_ms(20); //20ms
choutin 5:1c95260515c1 78
choutin 5:1c95260515c1 79 degree=160;
choutin 5:1c95260515c1 80
choutin 5:1c95260515c1 81 i=500+degree*1900/180;
choutin 5:1c95260515c1 82 pwmarm.write(i/PERIOD);
choutin 5:1c95260515c1 83
choutin 5:1c95260515c1 84
choutin 2:b204cf2f9b60 85 }
choutin 2:b204cf2f9b60 86
choutin 2:b204cf2f9b60 87
choutin 5:1c95260515c1 88
choutin 5:1c95260515c1 89 void open_hand(void) {
choutin 5:1c95260515c1 90 int i,degree;
choutin 5:1c95260515c1 91
choutin 5:1c95260515c1 92 pwmhand.period_ms(20); //20ms
choutin 5:1c95260515c1 93
choutin 5:1c95260515c1 94 degree=90;
choutin 5:1c95260515c1 95
choutin 5:1c95260515c1 96 i=500+degree*1900/180;
choutin 5:1c95260515c1 97 pwmhand.write(i/PERIOD);
choutin 5:1c95260515c1 98
choutin 5:1c95260515c1 99
choutin 5:1c95260515c1 100 }
choutin 5:1c95260515c1 101
choutin 5:1c95260515c1 102
choutin 5:1c95260515c1 103
choutin 5:1c95260515c1 104 void open_arm(void) {
choutin 5:1c95260515c1 105 int i,degree;
choutin 5:1c95260515c1 106
choutin 5:1c95260515c1 107 pwmarm.period_ms(20); //20ms
choutin 5:1c95260515c1 108
choutin 5:1c95260515c1 109 degree=10;
choutin 5:1c95260515c1 110
choutin 5:1c95260515c1 111 i=500+degree*1900/180;
choutin 5:1c95260515c1 112 pwmarm.write(i/PERIOD);
choutin 5:1c95260515c1 113
choutin 5:1c95260515c1 114
choutin 5:1c95260515c1 115 }
choutin 5:1c95260515c1 116
choutin 2:b204cf2f9b60 117 int main(){
choutin 5:1c95260515c1 118 move(-30,-30);
choutin 5:1c95260515c1 119 while(1){
choutin 5:1c95260515c1 120 open_arm();
choutin 5:1c95260515c1 121 close_hand();
choutin 5:1c95260515c1 122 move(1,1);
choutin 5:1c95260515c1 123 led=1;
choutin 5:1c95260515c1 124 wait(1);
choutin 5:1c95260515c1 125 close_arm();
choutin 5:1c95260515c1 126 open_hand();
choutin 5:1c95260515c1 127 move(-255,-255);
choutin 5:1c95260515c1 128 led=0;
choutin 5:1c95260515c1 129 wait(1);
choutin 5:1c95260515c1 130 }
choutin 5:1c95260515c1 131 return 0;
choutin 0:df2659fd8031 132 }