Tuk ZeroFour / Mbed 2 deprecated Kenya_2019

Tuk workshop

Dependencies:   mbed RC_Servo VMA306 PID CNY70 CMPS03 GP2A Pixy

main.cpp@18:96264a9ddaf6, 2019-11-01 (annotated)

Committer:
tuk4
Date:
Fri Nov 01 08:30:33 2019 +0000
Revision:
18:96264a9ddaf6
Parent:
17:961a91037658 
Committed on 1/11/2019 11:28 am

Who changed what in which revision?

UserRevisionLine numberNew contents of line
haarkon 0:4902b3f17137 1 #include "mbed.h"
haarkon 0:4902b3f17137 2 #include "CMPS03.h"
haarkon 0:4902b3f17137 3 #include "CNY70.h"
haarkon 0:4902b3f17137 4 #include "GP2A.h"
haarkon 0:4902b3f17137 5 #include "VMA306.h"
haarkon 3:0221daeeaa86 6 #include "Pixy.h"
haarkon 4:5038b4cd1088 7 #include "PID.h"
tuk4 10:6c3653c53eca 8 #include "motor_state.h"
haarkon 4:5038b4cd1088 9
tuk4 17:961a91037658 10
haarkon 4:5038b4cd1088 11 #define PI 3.1415926535898
haarkon 0:4902b3f17137 12
haarkon 8:2d4887624f94 13 Serial pc (PA_2, PA_3, 115200);
tuk4 11:31564089b41c 14
haarkon 7:6d58adc26b78 15 PID motor (TIM4, TIM3, PA_9, PA_8, PC_9, PC_8, PC_6, PC_5);
haarkon 0:4902b3f17137 16
haarkon 3:0221daeeaa86 17 CMPS03 boussole (PC_4);
haarkon 0:4902b3f17137 18
tuk4 17:961a91037658 19 CNY70 right_center (PC_3);
tuk4 17:961a91037658 20 CNY70 left_center (PC_2);
haarkon 3:0221daeeaa86 21 CNY70 exterior (PA_7);
haarkon 0:4902b3f17137 22
haarkon 8:2d4887624f94 23 VMA306 ultraSon (PB_15, PA_6, PB_14, PC_7, PB_13, PB_2);
haarkon 0:4902b3f17137 24
tuk4 11:31564089b41c 25 PIXY pixy (PA_0, PA_1, 50000);
haarkon 0:4902b3f17137 26
tuk4 10:6c3653c53eca 27 DigitalIn bp (PC_13);
haarkon 4:5038b4cd1088 28
haarkon 0:4902b3f17137 29 DigitalOut led1 (PA_5);
haarkon 0:4902b3f17137 30 DigitalOut led2 (PD_2);
haarkon 8:2d4887624f94 31 DigitalOut unused1 (PB_10);
haarkon 8:2d4887624f94 32 DigitalOut unused2 (PA_15);
haarkon 8:2d4887624f94 33 DigitalOut unused3 (PA_12);
haarkon 8:2d4887624f94 34
haarkon 8:2d4887624f94 35 DigitalIn unused4 (PA_4, PullUp);
haarkon 8:2d4887624f94 36 DigitalIn unused5 (PB_0, PullUp);
haarkon 8:2d4887624f94 37 DigitalIn unused6 (PC_1, PullUp);
haarkon 8:2d4887624f94 38 DigitalIn unused7 (PC_0, PullUp);
haarkon 8:2d4887624f94 39
haarkon 8:2d4887624f94 40 Timer temps;
haarkon 0:4902b3f17137 41
tuk4 10:6c3653c53eca 42 motor_state current_state = IDLE;
tuk4 10:6c3653c53eca 43
tuk4 9:4053b5217339 44 int main ()
haarkon 3:0221daeeaa86 45 {
tuk4 10:6c3653c53eca 46 int numberOfObjects=0, dummy=0;
tuk4 15:415c38c93b89 47 T_pixyNMBloc file;
tuk4 9:4053b5217339 48 motor.resetPosition();
tuk4 10:6c3653c53eca 49 double x=0, y=0, theta = 0;
tuk4 10:6c3653c53eca 50 motor.setProportionnalValue(2.0);
tuk4 10:6c3653c53eca 51 motor.setintegralValue(0.4);
tuk4 10:6c3653c53eca 52 motor.setDerivativeValue(1.0);
tuk4 14:899c6d29c0a8 53 double speed_L = -200, speed_R = -200;
tuk4 18:96264a9ddaf6 54 bool reset = false;
haarkon 3:0221daeeaa86 55 while (1) {
tuk4 15:415c38c93b89 56
tuk4 12:548cdc49cdba 57 motor.getPosition(&x, &y, &theta);
tuk4 15:415c38c93b89 58 pc.printf("\r state is %d x is %.2lf, y is %.2lf, theta is %.2lf",current_state,x,y,theta);
tuk4 15:415c38c93b89 59 switch (current_state) {
tuk4 15:415c38c93b89 60 case IDLE :
tuk4 15:415c38c93b89 61 motor.setSpeed(0,0);
tuk4 15:415c38c93b89 62 if (bp == 0) current_state = TURN_RIGHT;
tuk4 15:415c38c93b89 63 break;
tuk4 15:415c38c93b89 64
tuk4 15:415c38c93b89 65 case TURN_RIGHT :
tuk4 15:415c38c93b89 66 motor.setSpeed(-speed_L/10.0,speed_R/10.0);// turn right
tuk4 15:415c38c93b89 67
tuk4 15:415c38c93b89 68 if (pixy.checkNewImage()) {
tuk4 15:415c38c93b89 69 pixy.detectedObject(&numberOfObjects,&dummy);
tuk4 15:415c38c93b89 70 if (numberOfObjects==1) {
tuk4 15:415c38c93b89 71 current_state = LOOK_FOR_FILE;
tuk4 15:415c38c93b89 72 }
tuk4 12:548cdc49cdba 73 }
tuk4 15:415c38c93b89 74 break;
tuk4 15:415c38c93b89 75
tuk4 15:415c38c93b89 76 case SAFEMODE :
tuk4 15:415c38c93b89 77 motor.setSpeed(0,0);
tuk4 15:415c38c93b89 78 if (ultraSon.readUSB() >= 20) current_state = FORWARD;
tuk4 15:415c38c93b89 79 if (ultraSon.readUSB() <= 15) current_state = BACKWARD;
tuk4 15:415c38c93b89 80 break;
tuk4 15:415c38c93b89 81
tuk4 15:415c38c93b89 82 case FORWARD :
tuk4 12:548cdc49cdba 83 motor.setSpeed(speed_L,speed_R);
tuk4 12:548cdc49cdba 84 if (ultraSon.readUSB() <= 20) {
tuk4 12:548cdc49cdba 85 current_state = SAFEMODE;
tuk4 12:548cdc49cdba 86 }
tuk4 18:96264a9ddaf6 87 if (left_center.whatAmIOn()== 0 && reset == false)
tuk4 17:961a91037658 88 {
tuk4 17:961a91037658 89 led1==1;
tuk4 17:961a91037658 90 motor.resetPosition();
tuk4 18:96264a9ddaf6 91 reset = true;
tuk4 18:96264a9ddaf6 92
tuk4 17:961a91037658 93 }
tuk4 17:961a91037658 94 if (x<=-1300) current_state = STUPID1;
tuk4 15:415c38c93b89 95 break;
tuk4 15:415c38c93b89 96
tuk4 15:415c38c93b89 97 case BACKWARD :
tuk4 12:548cdc49cdba 98 motor.setSpeed(-speed_L,-speed_R);
tuk4 12:548cdc49cdba 99 if (ultraSon.readUSB() >= 15) current_state = SAFEMODE;
tuk4 15:415c38c93b89 100 break;
tuk4 15:415c38c93b89 101
tuk4 15:415c38c93b89 102 case LOOK_FOR_FILE :
tuk4 15:415c38c93b89 103 if (pixy.checkNewImage()) {
tuk4 15:415c38c93b89 104 pixy.detectedObject(&numberOfObjects,&dummy);
tuk4 15:415c38c93b89 105 if (numberOfObjects==1) {
tuk4 15:415c38c93b89 106 file=pixy.getNMBloc();
tuk4 15:415c38c93b89 107 pc.printf("\r x = %d", file.x);
tuk4 15:415c38c93b89 108 if (file.x >165) {
tuk4 15:415c38c93b89 109 motor.setSpeed(-speed_L/10.0,speed_R/10.0);// turn right
tuk4 15:415c38c93b89 110 pc.printf(" - Turning right");
tuk4 15:415c38c93b89 111 } else if (file.x <155) {
tuk4 15:415c38c93b89 112 motor.setSpeed(speed_L/10.0,-speed_R/10.0);// turn left
tuk4 15:415c38c93b89 113 pc.printf(" - Turning left");
tuk4 15:415c38c93b89 114 } else {
tuk4 15:415c38c93b89 115 motor.setSpeed(0,0);
tuk4 15:415c38c93b89 116 motor.resetPosition();
tuk4 15:415c38c93b89 117 current_state = FORWARD;
tuk4 15:415c38c93b89 118 }
tuk4 15:415c38c93b89 119 } else {
tuk4 15:415c38c93b89 120 //motor.setSpeed(-speed_L,speed_R);// turn right
tuk4 15:415c38c93b89 121 pc.printf(" - No object");
tuk4 15:415c38c93b89 122 }
tuk4 11:31564089b41c 123 }
tuk4 15:415c38c93b89 124 break;
tuk4 15:415c38c93b89 125
tuk4 15:415c38c93b89 126 case STUPID1 :
tuk4 15:415c38c93b89 127 wait(0.2);
tuk4 15:415c38c93b89 128 current_state = HARVEST1;
tuk4 15:415c38c93b89 129 break;
tuk4 15:415c38c93b89 130
tuk4 15:415c38c93b89 131 case STUPID2 :
tuk4 15:415c38c93b89 132 wait(0.2);
tuk4 15:415c38c93b89 133 current_state = HARVEST2;
tuk4 15:415c38c93b89 134 break;
tuk4 15:415c38c93b89 135
tuk4 15:415c38c93b89 136 case STUPID3 :
tuk4 15:415c38c93b89 137 wait(0.2);
tuk4 15:415c38c93b89 138 current_state = HARVEST3;
tuk4 15:415c38c93b89 139 break;
tuk4 15:415c38c93b89 140
tuk4 15:415c38c93b89 141
tuk4 15:415c38c93b89 142
tuk4 15:415c38c93b89 143 case HARVEST1 :
tuk4 15:415c38c93b89 144 motor.setSpeed(-speed_L,0.0);
tuk4 15:415c38c93b89 145 if (theta < (-2*PI)) current_state = STUPID2;
tuk4 15:415c38c93b89 146 break;
tuk4 15:415c38c93b89 147
tuk4 15:415c38c93b89 148 case HARVEST2 :
tuk4 15:415c38c93b89 149 motor.setSpeed(0.0,-speed_R);
tuk4 15:415c38c93b89 150 if (theta > 0) current_state = STUPID3;
tuk4 15:415c38c93b89 151 break;
tuk4 15:415c38c93b89 152
tuk4 15:415c38c93b89 153 case HARVEST3 :
tuk4 15:415c38c93b89 154 motor.setSpeed(speed_L,-speed_R);
tuk4 15:415c38c93b89 155 if (theta > (2*PI)) current_state = IDLE;
tuk4 15:415c38c93b89 156 break;
tuk4 15:415c38c93b89 157
tuk4 15:415c38c93b89 158 default :
tuk4 15:415c38c93b89 159 //do something
tuk4 15:415c38c93b89 160 break;
tuk4 10:6c3653c53eca 161 }
haarkon 3:0221daeeaa86 162 }
tuk4 15:415c38c93b89 163
tuk4 9:4053b5217339 164 return 0;
haarkon 0:4902b3f17137 165 }