Ephantus Mugo / Mbed 2 deprecated Kenya_2019

Test code 29-10-2019

Dependencies:   mbed RC_Servo VMA306 PID CNY70 CMPS03 GP2A Pixy

main.cpp@14:899c6d29c0a8, 2019-10-25 (annotated)

Committer:
tuk4
Date:
Fri Oct 25 13:53:17 2019 +0000
Revision:
14:899c6d29c0a8
Parent:
13:f81c8451d601
Child:
15:a0a88aeb4fe1
25/10/2019 1653 hrs

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