Feng Hong
/
Nucleo_rtos_basic
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Diff: payload.cpp
- Revision:
- 20:ec9d4f6a16ac
- Parent:
- 19:0356e54240cc
- Child:
- 21:5c6b3657c3cb
--- a/payload.cpp Tue Sep 24 14:05:58 2019 +0000
+++ b/payload.cpp Sat Oct 19 02:10:12 2019 +0000
@@ -25,6 +25,234 @@
extern int device_address; // last 8 bits of address
extern int device_type; // first 3 bits of adddress
+DigitalOut motor1_en(PB_11);
+DigitalOut motor1_dir(PB_2);
+DigitalOut motor1_pul(PB_1);
+
+DigitalOut motor2_en(PB_15);
+DigitalOut motor2_dir(PB_14);
+DigitalOut motor2_pul(PB_13);
+
+DigitalOut motor3_en(PC_6);
+DigitalOut motor3_dir(PB_6);
+DigitalOut motor3_pul(PB_12);
+
+int moveMotor1(int distance_mm, bool direction, bool freemove, bool gobackhome);
+int moveMotor2(int distance_mm, bool direction, bool freemove, bool gobackhome);
+int moveMotor3(int distance_mm, bool direction, bool freemove, bool gobackhome);
+
+int delay_us (int us)
+{
+ int ret;
+ int i, j;
+ for (i =0; i<us; i++)
+ {
+ for (j = 0; j <10000; j++)
+ {
+ ret++;
+ }
+ }
+ return ret;
+}
+int moveMotor(int motornumber, int distance_mm, bool direction, bool freemove, bool gobackhome)
+{
+ int ret = 0;
+ switch(motornumber) // 0: cuptrack0 1:cuptrack1 2:cuptrack2
+ {
+ case 1:
+ ret = moveMotor1(distance_mm, direction, freemove, gobackhome);
+ case 2:
+ ret = moveMotor2(distance_mm, direction, freemove, gobackhome);
+ case 3:
+ ret = moveMotor3(distance_mm, direction, freemove, gobackhome);
+ default:
+ return -1;
+ }
+ return ret;
+}
+int moveMotor1(int distance_mm, bool direction, bool freemove, bool gobackhome)
+{
+ int loop;
+ float mm_per_pulse = 143;
+ if (freemove)
+ {
+ //disable motor
+ motor1_en = 1;
+ printf("freemove\r\n");
+ return 0;
+ }
+ motor1_en = 0;
+ if (gobackhome)
+ {
+ //move cup to home
+ printf("go back home\r\n");
+ return 0;
+ }
+ else
+ {
+ printf("motor1 direction=%s distance_mm=%d\r\n", direction?"F":"B", distance_mm);
+ if (direction) // true: move forward false: move backward
+ {
+ //move forward
+ motor1_dir = 1;
+ delay_us(400);
+ for (loop = 0; loop < (int)(distance_mm*10000/mm_per_pulse); loop++)
+ {
+ motor1_pul = 0;
+ delay_us(400);
+// wait(0.00004);
+ motor1_pul = 1;
+ delay_us(400);
+// wait(0.00004);
+ }
+ }
+ else
+ {
+ //move backward
+ motor1_dir = 0;
+ delay_us(400);
+// wait(0.00001); //5ms
+ for (loop = 0; loop < (int)(distance_mm*10000/mm_per_pulse); loop++)
+ {
+ motor1_pul = 0;
+ delay_us(400);
+// wait(0.00003);
+ motor1_pul = 1;
+ delay_us(400);
+// wait(0.00003);
+ }
+ }
+ }
+}
+int moveMotor2(int distance_mm, bool direction, bool freemove, bool gobackhome)
+{
+ int loop;
+ float mm_per_pulse = 143;
+ if (freemove)
+ {
+ //disable motor
+ motor2_en = 1;
+ printf("freemove\r\n");
+ return 0;
+ }
+ motor2_en = 0;
+ if (gobackhome)
+ {
+ //move cup to home
+ printf("go back home\r\n");
+ return 0;
+ }
+ else
+ {
+ printf("motor2 direction=%s distance_mm=%d\r\n", direction?"F":"B", distance_mm);
+ if (direction) // true: move forward false: move backward
+ {
+ //move forward
+ motor2_dir = 1;
+ delay_us(400);
+ for (loop = 0; loop < (int)(distance_mm*10000/mm_per_pulse); loop++)
+ {
+ motor2_pul = 0;
+ delay_us(400);
+// wait(0.00004);
+ motor2_pul = 1;
+ delay_us(400);
+// wait(0.00004);
+ }
+ }
+ else
+ {
+ //move backward
+ motor2_dir = 0;
+ delay_us(400);
+// wait(0.00001); //5ms
+ for (loop = 0; loop < (int)(distance_mm*10000/mm_per_pulse); loop++)
+ {
+ motor2_pul = 0;
+ delay_us(400);
+// wait(0.00003);
+ motor2_pul = 1;
+ delay_us(400);
+// wait(0.00003);
+ }
+ }
+ }
+}
+int moveMotor3Until(bool direction, DigitalIn sensorpin)
+{
+ int distance = 0;
+ int mm_per_pulse = 143;
+ if (direction)
+ motor3_dir = 1;
+ else
+ motor3_dir = 0;
+ while(sensorpin.read() == 0)
+ {
+// printf("sensorpin %d\r\n", sensorpin.read());
+ distance++;
+ motor3_pul = 0;
+ delay_us(400);
+ //wait(0.00003);
+ motor3_pul = 1;
+ delay_us(400);
+ //wait(0.00003);
+ }
+ return (distance/mm_per_pulse);
+}
+int moveMotor3(int distance_mm, bool direction, bool freemove, bool gobackhome)
+{
+ int loop;
+ float mm_per_pulse = 143;
+ if (freemove)
+ {
+ //disable motor
+ motor3_en = 1;
+ printf("freemove\r\n");
+ return 0;
+ }
+ motor3_en = 0;
+ if (gobackhome)
+ {
+ //move cup to home
+ printf("go back home\r\n");
+ return 0;
+ }
+ else
+ {
+ printf("motor3 direction=%s distance_mm=%d\r\n", direction?"F":"B", distance_mm);
+ if (direction) // true: move forward false: move backward
+ {
+ //move forward
+ motor3_dir = 1;
+ delay_us(400);
+ for (loop = 0; loop < (int)(distance_mm*10000/mm_per_pulse); loop++)
+ {
+ motor3_pul = 0;
+ delay_us(400);
+// wait(0.00004);
+ motor3_pul = 1;
+ delay_us(400);
+// wait(0.00004);
+ }
+ }
+ else
+ {
+ //move backward
+ motor3_dir = 0;
+ delay_us(400);
+// wait(0.00001); //5ms
+ for (loop = 0; loop < (int)(distance_mm*10000/mm_per_pulse); loop++)
+ {
+ motor3_pul = 0;
+ delay_us(400);
+// wait(0.00003);
+ motor3_pul = 1;
+ delay_us(400);
+// wait(0.00003);
+ }
+ }
+ }
+}
void handleCupTrackCommand(data_field_d data_pack)
{
@@ -152,6 +380,7 @@
}
}
}
+ wait(0.0001);
}
