Diff: main.cpp

Revision:

0:def293862983

Child:

1:c828f7c35fae

diff -r 000000000000 -r def293862983 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Dec 10 06:33:25 2018 +0000
@@ -0,0 +1,126 @@
+#include "mbed.h"
+
+I2CSlave slave(PB_7,PB_6);
+
+
+DigitalOut InA1(D12);
+DigitalOut InB1(D11);
+PwmOut pwm1(D10);
+
+DigitalOut InA2(D9);
+DigitalOut InB2(D7);
+PwmOut pwm2(D3);
+
+DigitalOut InA3(D1);
+DigitalOut InB3(D0);
+PwmOut pwm3(D2);
+
+void rec_eve();
+void motor(int ch,int power);
+
+int main(){
+    wait(0.2);
+    slave.address(0x14);
+    pwm1.period(0.00052);
+    pwm2.period(0.00052);
+    pwm3.period(0.00052);
+    while (1) {
+        if(slave.receive() == I2CSlave::WriteAddressed){
+            rec_eve();
+        }
+    }
+}
+
+void rec_eve(){
+    char i2c_rec_buf[] = {0,0,0,0,0,0}; 
+    slave.read(i2c_rec_buf,6);
+    for(int i = 0; i< 6;i++){
+        if(i2c_rec_buf[i] == 0){
+            motor(i,0);
+        }
+        else if(i2c_rec_buf[i] == 128 ){
+            motor(i,999);
+        }
+        else if(i2c_rec_buf[i] < 128){
+            motor(i,i2c_rec_buf[i]);
+        }
+        else{
+            motor(i,-1*(i2c_rec_buf[i] - 128));
+        }
+    }
+    wait_ms(10);
+}
+
+void motor(int ch,int power){
+    
+    switch (ch){
+    case 0://right
+        if(power == 0){
+            InA1 = 0;
+            InB1 = 0;
+            pwm1 = 0;     
+        }
+        else if(power == 999){
+            InA1 = 1;
+            InB1 = 1;
+            pwm1 = 1;  
+        }
+        else if(power < 0){
+            InA1 = 0;
+            InB1 = 1;
+            pwm1 = -1*(float)power/100; 
+        }
+        else{
+            InA1 = 1;
+            InB1 = 0;
+            pwm1 = (float)power/100; 
+        }
+        break;
+    case 5://back
+        if(power == 0){
+            InA2 = 0;
+            InB2 = 0;
+            pwm2 = 0;     
+        }
+        else if(power == 999){
+            InA2 = 1;
+            InB2 = 1;
+            pwm2 = 1;  
+        }
+        else if(power < 0){
+            InA2 = 0;
+            InB2 = 1;
+            pwm2 = -1*(float)power/100; 
+        }
+        else{
+            InA2 = 1;
+            InB2 = 0;
+            pwm2 = (float)power/100; 
+        }
+        break;
+    case 2://left
+        if(power == 0){
+            InA3 = 0;
+            InB3 = 0;
+            pwm3 = 0;     
+        }
+        else if(power == 999){
+            InA3 = 1;
+            InB3 = 1;
+            pwm3 = 1;  
+        }
+        else if(power < 0){
+            InA3 = 0;
+            InB3 = 1;
+            pwm3 = -1*(float)power/100; 
+        }
+        else{
+            InA3 = 1;
+            InB3 = 0;
+            pwm3 = (float)power/100; 
+        }
+        break;
+    default:
+        break;
+    }
+}    
\ No newline at end of file