Initial Commit
Dependencies: mbed HC05 QEI MODSERIAL SWSPI mbed-rtos
main.cpp@3:d1197b5ea68a, 2014-10-12 (annotated)
- Committer:
- Throwbot
- Date:
- Sun Oct 12 23:27:43 2014 +0000
- Revision:
- 3:d1197b5ea68a
- Parent:
- 2:0f80c8a1c236
- Child:
- 4:81b0de07841f
Ebot demo working
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Throwbot | 1:1da89c13dfa1 | 1 | // FORMAT_CODE_START |
Throwbot | 0:43331220df0d | 2 | #include "QEI.h" |
Throwbot | 0:43331220df0d | 3 | #include "mbed.h" |
Throwbot | 0:43331220df0d | 4 | #include "robot.h" |
Throwbot | 0:43331220df0d | 5 | #include "rtos.h" |
Throwbot | 1:1da89c13dfa1 | 6 | #include "MPU6050.h" |
Throwbot | 1:1da89c13dfa1 | 7 | #include "I2Cdev.h" |
Throwbot | 1:1da89c13dfa1 | 8 | #include "tone.h" |
Throwbot | 1:1da89c13dfa1 | 9 | #include "ultrasonic.h" |
Throwbot | 2:0f80c8a1c236 | 10 | #include "bt_shell.h" |
Throwbot | 2:0f80c8a1c236 | 11 | int Current_Right_pulse=0; |
Throwbot | 2:0f80c8a1c236 | 12 | int Current_Left_pulse=0; |
Throwbot | 2:0f80c8a1c236 | 13 | int Linput=0; |
Throwbot | 2:0f80c8a1c236 | 14 | int Rinput=0; |
Throwbot | 2:0f80c8a1c236 | 15 | int Change_Right_pulse=0; |
Throwbot | 2:0f80c8a1c236 | 16 | int Change_Left_pulse=0; |
Throwbot | 2:0f80c8a1c236 | 17 | int Previous_Left_pulse=0; |
Throwbot | 2:0f80c8a1c236 | 18 | int Previos_Right_pulse=0; |
Throwbot | 2:0f80c8a1c236 | 19 | int time_int = 50; |
Throwbot | 2:0f80c8a1c236 | 20 | int time_factor=1000/time_int ; |
Throwbot | 3:d1197b5ea68a | 21 | int Error_Right=0; |
Throwbot | 3:d1197b5ea68a | 22 | int Error_Left=0; |
Throwbot | 3:d1197b5ea68a | 23 | float L_Kp=0.1; |
Throwbot | 3:d1197b5ea68a | 24 | float R_Kp=0.1; |
Throwbot | 3:d1197b5ea68a | 25 | int previous_Linput= 0 ; |
Throwbot | 3:d1197b5ea68a | 26 | int previous_Rinput= 0 ; |
Throwbot | 2:0f80c8a1c236 | 27 | RtosTimer *Motor_controller_Timer; |
Throwbot | 2:0f80c8a1c236 | 28 | |
Throwbot | 2:0f80c8a1c236 | 29 | void Motorcontroller(void const *args) |
Throwbot | 2:0f80c8a1c236 | 30 | { |
Throwbot | 2:0f80c8a1c236 | 31 | |
Throwbot | 3:d1197b5ea68a | 32 | Current_Right_pulse= right.getPulses()/5; |
Throwbot | 3:d1197b5ea68a | 33 | Current_Left_pulse=left.getPulses()*(-1)/5; |
Throwbot | 2:0f80c8a1c236 | 34 | |
Throwbot | 2:0f80c8a1c236 | 35 | Change_Right_pulse=Current_Right_pulse- Previos_Right_pulse; |
Throwbot | 2:0f80c8a1c236 | 36 | Change_Left_pulse=Current_Left_pulse- Previous_Left_pulse; |
Throwbot | 2:0f80c8a1c236 | 37 | |
Throwbot | 2:0f80c8a1c236 | 38 | Previous_Left_pulse=Current_Left_pulse; |
Throwbot | 2:0f80c8a1c236 | 39 | Previos_Right_pulse=Current_Right_pulse; |
Throwbot | 2:0f80c8a1c236 | 40 | |
Throwbot | 3:d1197b5ea68a | 41 | Error_Right=(Rmotor_speed- (Change_Right_pulse*time_factor) ); |
Throwbot | 3:d1197b5ea68a | 42 | Error_Left=(Lmotor_speed- (Change_Left_pulse*time_factor)); |
Throwbot | 2:0f80c8a1c236 | 43 | Linput=(Linput+Error_Left*L_Kp); |
Throwbot | 2:0f80c8a1c236 | 44 | Rinput=(Rinput+Error_Right*R_Kp); |
Throwbot | 2:0f80c8a1c236 | 45 | if(Linput>100) |
Throwbot | 2:0f80c8a1c236 | 46 | Linput=100; |
Throwbot | 2:0f80c8a1c236 | 47 | else if (Linput<-100) |
Throwbot | 2:0f80c8a1c236 | 48 | Linput= -100; |
Throwbot | 3:d1197b5ea68a | 49 | else if (-11<Linput && Linput<11) |
Throwbot | 2:0f80c8a1c236 | 50 | Linput= 0; |
Throwbot | 2:0f80c8a1c236 | 51 | if(Rinput>100) |
Throwbot | 2:0f80c8a1c236 | 52 | Rinput=100; |
Throwbot | 2:0f80c8a1c236 | 53 | else if(Rinput<-100) |
Throwbot | 2:0f80c8a1c236 | 54 | Rinput= -100; |
Throwbot | 3:d1197b5ea68a | 55 | else if (-11<Rinput && Rinput<11) |
Throwbot | 2:0f80c8a1c236 | 56 | Rinput= 0; |
Throwbot | 3:d1197b5ea68a | 57 | if (Linput== previous_Linput && Rinput ==previous_Rinput) { |
Throwbot | 3:d1197b5ea68a | 58 | } else { |
Throwbot | 3:d1197b5ea68a | 59 | previous_Linput= Linput ; |
Throwbot | 3:d1197b5ea68a | 60 | previous_Rinput= Rinput ; |
Throwbot | 3:d1197b5ea68a | 61 | motor_control(-Rinput,-Linput); |
Throwbot | 3:d1197b5ea68a | 62 | } |
Throwbot | 3:d1197b5ea68a | 63 | /* |
Throwbot | 3:d1197b5ea68a | 64 | bt.lock(); |
Throwbot | 3:d1197b5ea68a | 65 | bt.printf(">>D;%d;%d;%d;%d;%d;%d;%d;%d;\r\n", |
Throwbot | 3:d1197b5ea68a | 66 | Rmotor_speed,Lmotor_speed,Error_Right ,Error_Left,\ |
Throwbot | 3:d1197b5ea68a | 67 | Rinput,Linput,Change_Right_pulse,Change_Left_pulse); |
Throwbot | 3:d1197b5ea68a | 68 | bt.unlock(); |
Throwbot | 3:d1197b5ea68a | 69 | */ |
Throwbot | 2:0f80c8a1c236 | 70 | } |
Throwbot | 2:0f80c8a1c236 | 71 | |
Throwbot | 2:0f80c8a1c236 | 72 | |
Throwbot | 0:43331220df0d | 73 | char c; |
Throwbot | 1:1da89c13dfa1 | 74 | char buffer[10]; |
Throwbot | 2:0f80c8a1c236 | 75 | void bt_shell_thr(void const *args) |
Throwbot | 2:0f80c8a1c236 | 76 | { |
Throwbot | 2:0f80c8a1c236 | 77 | while(true) { |
Throwbot | 2:0f80c8a1c236 | 78 | bt_shell_run(); |
Throwbot | 2:0f80c8a1c236 | 79 | Thread::wait(50); |
Throwbot | 2:0f80c8a1c236 | 80 | } |
Throwbot | 2:0f80c8a1c236 | 81 | } |
Throwbot | 1:1da89c13dfa1 | 82 | |
Throwbot | 1:1da89c13dfa1 | 83 | int main() |
Throwbot | 1:1da89c13dfa1 | 84 | { |
Throwbot | 1:1da89c13dfa1 | 85 | initRobot(); |
Throwbot | 1:1da89c13dfa1 | 86 | while(buffer[3] != 'O') { |
Throwbot | 1:1da89c13dfa1 | 87 | while(buffer[3] != 'E') { |
Throwbot | 1:1da89c13dfa1 | 88 | if(bt.readable()) { |
Throwbot | 1:1da89c13dfa1 | 89 | bt.gets(buffer, 5); |
Throwbot | 1:1da89c13dfa1 | 90 | if(buffer[3] == 'E') { |
Throwbot | 1:1da89c13dfa1 | 91 | bt.printf(">>1B"); |
Throwbot | 1:1da89c13dfa1 | 92 | } else if(buffer[3] == '?') { |
Throwbot | 2:0f80c8a1c236 | 93 | bt.printf("eBot#2"); |
Throwbot | 1:1da89c13dfa1 | 94 | } |
Throwbot | 1:1da89c13dfa1 | 95 | } |
Throwbot | 1:1da89c13dfa1 | 96 | } |
Throwbot | 1:1da89c13dfa1 | 97 | if(bt.readable()) { |
Throwbot | 1:1da89c13dfa1 | 98 | bt.gets(buffer, 5); |
Throwbot | 1:1da89c13dfa1 | 99 | } |
Throwbot | 1:1da89c13dfa1 | 100 | } |
Throwbot | 1:1da89c13dfa1 | 101 | //imperial_march(); |
Throwbot | 1:1da89c13dfa1 | 102 | Thread IMU_th(Imu_yaw, NULL, osPriorityNormal, 2048, NULL); |
Throwbot | 1:1da89c13dfa1 | 103 | Thread Encoder_th(encoder_thread, NULL, osPriorityNormal, 2048, NULL); |
Throwbot | 2:0f80c8a1c236 | 104 | Thread bt_shell_th(bt_shell_thr, NULL, osPriorityNormal, 2048, NULL); |
Throwbot | 2:0f80c8a1c236 | 105 | Motor_controller_Timer = new RtosTimer(&Motorcontroller, osTimerPeriodic, NULL); |
Throwbot | 3:d1197b5ea68a | 106 | Motor_controller_Timer->start(time_int); |
Throwbot | 1:1da89c13dfa1 | 107 | while(1) { |
Throwbot | 1:1da89c13dfa1 | 108 | if(ldrread1()>0.6) { |
Throwbot | 1:1da89c13dfa1 | 109 | } |
Throwbot | 1:1da89c13dfa1 | 110 | if(ldrread2()>0.6) { |
Throwbot | 1:1da89c13dfa1 | 111 | Led_on(); |
Throwbot | 1:1da89c13dfa1 | 112 | } else { |
Throwbot | 1:1da89c13dfa1 | 113 | Led_off(); |
Throwbot | 1:1da89c13dfa1 | 114 | } |
Throwbot | 2:0f80c8a1c236 | 115 | ultrasonic_run(); |
Throwbot | 2:0f80c8a1c236 | 116 | Thread::yield(); |
Throwbot | 0:43331220df0d | 117 | } |
Throwbot | 2:0f80c8a1c236 | 118 | } |