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?
User | Revision | Line number | New 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 | } |