Ohnishi_Gundan / Mbed 2 deprecated Master-FM

FM-test

Dependencies:   MODSERIAL mbed-rtos mbed

Fork of Master by Ohnishi_Gundan

main.cpp@13:3f0505bbe284, 2014-09-15 (annotated)

Committer:
9uS7
Date:
Mon Sep 15 04:05:56 2014 +0000
Revision:
13:3f0505bbe284
Parent:
12:168338a29373 
add fmStop and fmRestart

Who changed what in which revision?

UserRevisionLine numberNew contents of line
9uS7 0:4f07ba929908 1 //Master mbed Program
9uS7 0:4f07ba929908 2
9uS7 0:4f07ba929908 3 #include "mbed.h"
9uS7 0:4f07ba929908 4 #include "fm.h"
9uS7 0:4f07ba929908 5 #include "bluetooth.h"
9uS7 1:e1cfb5850088 6 #include "control.h"
9uS7 0:4f07ba929908 7
9uS7 1:e1cfb5850088 8 //BT_MASTER or BT_SLAVE
9uS7 12:168338a29373 9 #define DEVICE_ROLE BT_MASTER
9uS7 0:4f07ba929908 10
9uS7 4:aaaadb45cbd9 11 //Serial pc(USBTX, USBRX); // tx, rx
9uS7 12:168338a29373 12
9uS7 12:168338a29373 13 AnalogIn mic(p17);
9uS7 0:4f07ba929908 14
9uS7 0:4f07ba929908 15 //debug
9uS7 0:4f07ba929908 16 DigitalOut led1(LED1);
9uS7 0:4f07ba929908 17 DigitalOut led2(LED2);
9uS7 0:4f07ba929908 18 DigitalOut led4(LED4);
9uS7 5:37733f175430 19
9uS7 0:4f07ba929908 20
9uS7 3:12e1f116ea42 21 void masterLoop(void);
9uS7 0:4f07ba929908 22
9uS7 0:4f07ba929908 23 int main()
9uS7 0:4f07ba929908 24 {
9uS7 1:e1cfb5850088 25 //FM_FREQUENCY is defined in fm.h
9uS7 1:e1cfb5850088 26 unsigned int fm_frequency = DEVICE_ROLE==BT_MASTER ? FM_FREQUENCY1 : FM_FREQUENCY2;
9uS7 1:e1cfb5850088 27
9uS7 11:b8d46d371937 28 fmSetup( fm_frequency );
9uS7 11:b8d46d371937 29 //btSetup(DEVICE_ROLE);
9uS7 3:12e1f116ea42 30 //motorSetup();
9uS7 2:c610e1a7fbcd 31
9uS7 0:4f07ba929908 32 while(1){
9uS7 3:12e1f116ea42 33 if( DEVICE_ROLE==BT_MASTER ){
9uS7 11:b8d46d371937 34 //masterLoop();
9uS7 3:12e1f116ea42 35 }
9uS7 13:3f0505bbe284 36 fmStop();
9uS7 13:3f0505bbe284 37 wait(2);
9uS7 13:3f0505bbe284 38 fmRestart();
9uS7 13:3f0505bbe284 39 wait(2);
9uS7 0:4f07ba929908 40 }
9uS7 0:4f07ba929908 41 }
9uS7 0:4f07ba929908 42
9uS7 3:12e1f116ea42 43 void masterLoop(){
9uS7 3:12e1f116ea42 44 float ir_m,fsr_m; //sensor of master
9uS7 3:12e1f116ea42 45 float ir_s,fsr_s; //sensor of slave
9uS7 9:6057314dc8ec 46 char function_m; //motor function of master
9uS7 9:6057314dc8ec 47 char function_s; //motor function of slave
9uS7 9:6057314dc8ec 48 float power_m; //motor power of master
9uS7 9:6057314dc8ec 49 float power_s; //motor power of slave
9uS7 9:6057314dc8ec 50 float dst;
9uS7 9:6057314dc8ec 51 getSensor( &ir_m, &fsr_m );
9uS7 6:df6d8ba1907a 52 recieveSensor( &ir_s, &fsr_s );
9uS7 9:6057314dc8ec 53
9uS7 9:6057314dc8ec 54 //距離を一定に保つ
9uS7 9:6057314dc8ec 55 /*
9uS7 9:6057314dc8ec 56 if( (dst=abs( ir_m+ir_s - center_pos*2 ))<0.1 ){
9uS7 9:6057314dc8ec 57 if( fsr_s-fsr_m>0.2 ){
9uS7 9:6057314dc8ec 58 function_m = MOTOR_OPEN;
9uS7 9:6057314dc8ec 59 power_m = 0;
9uS7 9:6057314dc8ec 60 function_s = MOTOR_PULL;
9uS7 9:6057314dc8ec 61 power_s = dst/2;
9uS7 9:6057314dc8ec 62 }
9uS7 9:6057314dc8ec 63 else if( fsr_m-fsr_s >0.2 ){
9uS7 9:6057314dc8ec 64 function_m = MOTOR_PULL;
9uS7 9:6057314dc8ec 65 power_m = dst/2;
9uS7 9:6057314dc8ec 66 function_s = MOTOR_OPEN;
9uS7 9:6057314dc8ec 67 power_s = 0;
9uS7 9:6057314dc8ec 68 }
9uS7 9:6057314dc8ec 69 else{
9uS7 9:6057314dc8ec 70 function_m = MOTOR_OPEN;
9uS7 9:6057314dc8ec 71 power_m = 0;
9uS7 9:6057314dc8ec 72 function_s = MOTOR_OPEN;
9uS7 9:6057314dc8ec 73 power_s = 0;
9uS7 9:6057314dc8ec 74 }
9uS7 9:6057314dc8ec 75 }
9uS7 9:6057314dc8ec 76 else if( ir_m+ir_s - center_pos*2 < 0 ){
9uS7 9:6057314dc8ec 77 function_m = MOTOR_LOOSE;
9uS7 9:6057314dc8ec 78 power_m = dst/2;
9uS7 9:6057314dc8ec 79 function_s = MOTOR_LOOSE;
9uS7 9:6057314dc8ec 80 power_s = dst/2;
9uS7 9:6057314dc8ec 81 }
9uS7 9:6057314dc8ec 82 else{
9uS7 9:6057314dc8ec 83 function_m = MOTOR_PULL;
9uS7 9:6057314dc8ec 84 power_m = dst/2;
9uS7 9:6057314dc8ec 85 function_s = MOTOR_PULL;
9uS7 9:6057314dc8ec 86 power_s = dst/2;
9uS7 9:6057314dc8ec 87 }
9uS7 9:6057314dc8ec 88 */
9uS7 9:6057314dc8ec 89
9uS7 9:6057314dc8ec 90 function_m = MOTOR_PULL;
9uS7 9:6057314dc8ec 91 power_m = 0.1;
9uS7 9:6057314dc8ec 92 function_s = MOTOR_PULL;
9uS7 9:6057314dc8ec 93 power_s = 0.1;
9uS7 9:6057314dc8ec 94
9uS7 9:6057314dc8ec 95
9uS7 9:6057314dc8ec 96 //Master's motor
9uS7 10:a90935ea0a4b 97 motor( function_m, power_m );
9uS7 9:6057314dc8ec 98 //slave's motor
9uS7 9:6057314dc8ec 99 sendMotor( function_s, power_s );
9uS7 9:6057314dc8ec 100
9uS7 4:aaaadb45cbd9 101 led4 = ( ir_s>0.5 ? 1 : 0 );
9uS7 4:aaaadb45cbd9 102 led2 = ( led2 ? 0 : 1 );
9uS7 9:6057314dc8ec 103 wait(0.1);
9uS7 3:12e1f116ea42 104 }