Zhan Tu / Mbed 2 deprecated Fish_2014Fall

Fish with BBBcamshift

Dependencies:   L3G4200D L3GD20 LSM303DLHC LSM303DLM PwmIn Servo mbed

main.cpp@0:8f37781c0054, 2015-04-17 (annotated)

Committer:
tzxl10000
Date:
Fri Apr 17 21:13:44 2015 +0000
Revision:
0:8f37781c0054
Fish_2014 with BBBCamshift

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tzxl10000 0:8f37781c0054 1 #include "mbed.h"
tzxl10000 0:8f37781c0054 2 #include "L3GD20.h"
tzxl10000 0:8f37781c0054 3 #include "LSM303DLHC.h"
tzxl10000 0:8f37781c0054 4 #include "Servo.h"
tzxl10000 0:8f37781c0054 5 #include "math.h"
tzxl10000 0:8f37781c0054 6 #include "PwmIn.h"
tzxl10000 0:8f37781c0054 7
tzxl10000 0:8f37781c0054 8 #include "iSerial.h"
tzxl10000 0:8f37781c0054 9 #include "iostream"
tzxl10000 0:8f37781c0054 10 #include "stdio.h"
tzxl10000 0:8f37781c0054 11 #include "string.h"
tzxl10000 0:8f37781c0054 12
tzxl10000 0:8f37781c0054 13 #include <string>
tzxl10000 0:8f37781c0054 14 #include <sstream>
tzxl10000 0:8f37781c0054 15 #define TS 10 // sample time [ms]
tzxl10000 0:8f37781c0054 16 #define SIMULATION_TIME 10 // simulation time [s]
tzxl10000 0:8f37781c0054 17
tzxl10000 0:8f37781c0054 18
tzxl10000 0:8f37781c0054 19
tzxl10000 0:8f37781c0054 20 //serial
tzxl10000 0:8f37781c0054 21 Serial BB(p13, p14); // tx, rx
tzxl10000 0:8f37781c0054 22 Serial pc(USBTX,USBRX);
tzxl10000 0:8f37781c0054 23
tzxl10000 0:8f37781c0054 24 //period_pwm setup as 20ms
tzxl10000 0:8f37781c0054 25 //speed: 0 is off, and 1 is full speed；for servo 0-0.2 duty circle
tzxl10000 0:8f37781c0054 26 //direction: 0 clockwise, 1 counter-clockwise
tzxl10000 0:8f37781c0054 27
tzxl10000 0:8f37781c0054 28 float period_pwm_ac; // Actuator current
tzxl10000 0:8f37781c0054 29 float period_pwm;
tzxl10000 0:8f37781c0054 30
tzxl10000 0:8f37781c0054 31 bool directionA;
tzxl10000 0:8f37781c0054 32 bool directionB;
tzxl10000 0:8f37781c0054 33 bool directionC;
tzxl10000 0:8f37781c0054 34
tzxl10000 0:8f37781c0054 35 float speedA;
tzxl10000 0:8f37781c0054 36 float speedB;
tzxl10000 0:8f37781c0054 37 float speedC;
tzxl10000 0:8f37781c0054 38
tzxl10000 0:8f37781c0054 39 int flag;
tzxl10000 0:8f37781c0054 40 int x,y; // Image coordinate
tzxl10000 0:8f37781c0054 41
tzxl10000 0:8f37781c0054 42 //funtion declaration
tzxl10000 0:8f37781c0054 43 void move(int motor, float speed, int direction);
tzxl10000 0:8f37781c0054 44 PwmOut PWMA(p25);//LEFT FIN
tzxl10000 0:8f37781c0054 45 PwmOut PWMB(p23);//RIGHT FIN
tzxl10000 0:8f37781c0054 46 PwmOut PWMC(p24);//Left servo
tzxl10000 0:8f37781c0054 47 PwmOut PWMD(p26);//Right servo
tzxl10000 0:8f37781c0054 48 PwmOut PWME(p21);//Tail
tzxl10000 0:8f37781c0054 49
tzxl10000 0:8f37781c0054 50 DigitalOut STBY (p30);
tzxl10000 0:8f37781c0054 51 //DigitalOut STBY2 (p29);
tzxl10000 0:8f37781c0054 52 DigitalOut AIN1 (p8);
tzxl10000 0:8f37781c0054 53 DigitalOut AIN2 (p11); // Fin1 left
tzxl10000 0:8f37781c0054 54 DigitalOut BIN1 (p7);
tzxl10000 0:8f37781c0054 55 DigitalOut BIN2 (p6); // Fin2 right
tzxl10000 0:8f37781c0054 56 DigitalOut myLed (LED1);
tzxl10000 0:8f37781c0054 57 DigitalOut myLed1(LED2);
tzxl10000 0:8f37781c0054 58
tzxl10000 0:8f37781c0054 59
tzxl10000 0:8f37781c0054 60
tzxl10000 0:8f37781c0054 61
tzxl10000 0:8f37781c0054 62 void moveA()
tzxl10000 0:8f37781c0054 63 {
tzxl10000 0:8f37781c0054 64 STBY=1; //disable standby
tzxl10000 0:8f37781c0054 65 int inPin1=1;
tzxl10000 0:8f37781c0054 66 int inPin2=0;
tzxl10000 0:8f37781c0054 67 if(directionA==0) {
tzxl10000 0:8f37781c0054 68 inPin1 = 0;
tzxl10000 0:8f37781c0054 69 inPin2 = 1;
tzxl10000 0:8f37781c0054 70 }
tzxl10000 0:8f37781c0054 71 AIN1=inPin1;
tzxl10000 0:8f37781c0054 72 AIN2=inPin2;
tzxl10000 0:8f37781c0054 73 directionA=!directionA;
tzxl10000 0:8f37781c0054 74
tzxl10000 0:8f37781c0054 75 //pc.printf("dirA = %d\n\r",directionA);
tzxl10000 0:8f37781c0054 76
tzxl10000 0:8f37781c0054 77
tzxl10000 0:8f37781c0054 78
tzxl10000 0:8f37781c0054 79 PWMA.pulsewidth(period_pwm*speedA);
tzxl10000 0:8f37781c0054 80
tzxl10000 0:8f37781c0054 81 }
tzxl10000 0:8f37781c0054 82
tzxl10000 0:8f37781c0054 83 void moveB()
tzxl10000 0:8f37781c0054 84 {
tzxl10000 0:8f37781c0054 85 STBY=1; //disable standby
tzxl10000 0:8f37781c0054 86 int inPin1=0;
tzxl10000 0:8f37781c0054 87 int inPin2=1;
tzxl10000 0:8f37781c0054 88 if(directionB==0) {
tzxl10000 0:8f37781c0054 89 inPin1 = 1;
tzxl10000 0:8f37781c0054 90 inPin2 = 0;
tzxl10000 0:8f37781c0054 91 }
tzxl10000 0:8f37781c0054 92 //pc.printf("dirB = %d\n\r",directionB);
tzxl10000 0:8f37781c0054 93 BIN1=inPin1;
tzxl10000 0:8f37781c0054 94 BIN2=inPin2;
tzxl10000 0:8f37781c0054 95 directionB=!directionB;
tzxl10000 0:8f37781c0054 96
tzxl10000 0:8f37781c0054 97 PWMB.pulsewidth(period_pwm*speedB);
tzxl10000 0:8f37781c0054 98
tzxl10000 0:8f37781c0054 99 }
tzxl10000 0:8f37781c0054 100
tzxl10000 0:8f37781c0054 101 void stop()
tzxl10000 0:8f37781c0054 102 {
tzxl10000 0:8f37781c0054 103 //enable standby
tzxl10000 0:8f37781c0054 104 STBY=0;;
tzxl10000 0:8f37781c0054 105 }
tzxl10000 0:8f37781c0054 106
tzxl10000 0:8f37781c0054 107 int main()
tzxl10000 0:8f37781c0054 108 {
tzxl10000 0:8f37781c0054 109 //serial to BBB setup
tzxl10000 0:8f37781c0054 110 char str[9];
tzxl10000 0:8f37781c0054 111 char *token;
tzxl10000 0:8f37781c0054 112
tzxl10000 0:8f37781c0054 113 pc.baud(9600);
tzxl10000 0:8f37781c0054 114 BB.baud(9600);
tzxl10000 0:8f37781c0054 115
tzxl10000 0:8f37781c0054 116 //Actuator & servo setup
tzxl10000 0:8f37781c0054 117
tzxl10000 0:8f37781c0054 118 period_pwm_ac=0.0020; //500hz
tzxl10000 0:8f37781c0054 119 period_pwm=0.020; //20ms
tzxl10000 0:8f37781c0054 120 PWMA.period(period_pwm_ac);
tzxl10000 0:8f37781c0054 121 PWMB.period(period_pwm_ac);
tzxl10000 0:8f37781c0054 122 PWMC.period(period_pwm); // servo requires a 20ms period
tzxl10000 0:8f37781c0054 123 PWMD.period(period_pwm); // servo requires a 20ms period
tzxl10000 0:8f37781c0054 124 PWME.period(period_pwm); // servo requires a 20ms period
tzxl10000 0:8f37781c0054 125
tzxl10000 0:8f37781c0054 126 //initial direction & current
tzxl10000 0:8f37781c0054 127 directionA=1;
tzxl10000 0:8f37781c0054 128 directionB=0;
tzxl10000 0:8f37781c0054 129 flag=1;
tzxl10000 0:8f37781c0054 130 speedA=0.2;
tzxl10000 0:8f37781c0054 131 speedB=0.2; //Actuator speed control
tzxl10000 0:8f37781c0054 132
tzxl10000 0:8f37781c0054 133 while(1)
tzxl10000 0:8f37781c0054 134 {
tzxl10000 0:8f37781c0054 135
tzxl10000 0:8f37781c0054 136 //Talk to BBB
tzxl10000 0:8f37781c0054 137 int i;
tzxl10000 0:8f37781c0054 138 if(BB.readable()>0)
tzxl10000 0:8f37781c0054 139 {
tzxl10000 0:8f37781c0054 140 for(i=0;i<9;i++)
tzxl10000 0:8f37781c0054 141 str[i] = BB.getc();
tzxl10000 0:8f37781c0054 142
tzxl10000 0:8f37781c0054 143 if((0x30<str[1]<35)&&(str[8]==0x0D)&&(str[3]==0x2C))
tzxl10000 0:8f37781c0054 144 {
tzxl10000 0:8f37781c0054 145 token = strtok(str, ",");
tzxl10000 0:8f37781c0054 146 x = atoi(token); //100-420(width_320);No target:555
tzxl10000 0:8f37781c0054 147
tzxl10000 0:8f37781c0054 148 token = strtok(NULL, ",");
tzxl10000 0:8f37781c0054 149 y = atoi(token); //100-340(height_240); No target:555
tzxl10000 0:8f37781c0054 150 // pc.printf(" x=%d y=%d", x,y);
tzxl10000 0:8f37781c0054 151 //pc.printf(str);
tzxl10000 0:8f37781c0054 152 }
tzxl10000 0:8f37781c0054 153 }
tzxl10000 0:8f37781c0054 154
tzxl10000 0:8f37781c0054 155 //Serial test
tzxl10000 0:8f37781c0054 156 if(x>210)
tzxl10000 0:8f37781c0054 157 moveA();
tzxl10000 0:8f37781c0054 158 else if (x<210)
tzxl10000 0:8f37781c0054 159 moveB();
tzxl10000 0:8f37781c0054 160 else if (x==555)
tzxl10000 0:8f37781c0054 161 {
tzxl10000 0:8f37781c0054 162 moveA();
tzxl10000 0:8f37781c0054 163 moveB();
tzxl10000 0:8f37781c0054 164 wait(0.2);
tzxl10000 0:8f37781c0054 165 }
tzxl10000 0:8f37781c0054 166
tzxl10000 0:8f37781c0054 167
tzxl10000 0:8f37781c0054 168 //Actuator test
tzxl10000 0:8f37781c0054 169 // moveA();
tzxl10000 0:8f37781c0054 170 // moveB();
tzxl10000 0:8f37781c0054 171 // wait(0.2);
tzxl10000 0:8f37781c0054 172
tzxl10000 0:8f37781c0054 173 //Servo test
tzxl10000 0:8f37781c0054 174
tzxl10000 0:8f37781c0054 175 // PWMC.pulsewidth(period_pwm*0.08);
tzxl10000 0:8f37781c0054 176 // PWMD.pulsewidth(period_pwm*0.08);
tzxl10000 0:8f37781c0054 177 // PWME.pulsewidth(period_pwm*0.08);
tzxl10000 0:8f37781c0054 178 // wait(0.05);
tzxl10000 0:8f37781c0054 179 // PWMC.pulsewidth(period_pwm*0.05);
tzxl10000 0:8f37781c0054 180 // PWMD.pulsewidth(period_pwm*0.05);
tzxl10000 0:8f37781c0054 181 // PWME.pulsewidth(period_pwm*0.08);
tzxl10000 0:8f37781c0054 182 //
tzxl10000 0:8f37781c0054 183
tzxl10000 0:8f37781c0054 184
tzxl10000 0:8f37781c0054 185
tzxl10000 0:8f37781c0054 186
tzxl10000 0:8f37781c0054 187 }
tzxl10000 0:8f37781c0054 188
tzxl10000 0:8f37781c0054 189 }