Spirit Dagozilla / Mbed OS BaseControlF7

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.

Dependencies:   dagozilla_utils_isaac cmps_dagoz motor_dagoz BaseControlF7 encoder_dagoz EncoderMotorInterrupt ros_lib_melodic_test

Dependents:   BaseControlF7

main.cpp@2:830d3c808679, 2019-03-23 (annotated)

Committer:
calmantara186
Date:
Sat Mar 23 09:48:27 2019 +0000
Revision:
2:830d3c808679
Parent:
1:8b6487805a90
Child:
3:0521855e7337
kontrol buat F7

Who changed what in which revision?

UserRevisionLine numberNew contents of line
calmantara186 0:1c22457d4aed 1 #include "mbed.h"
calmantara186 1:8b6487805a90 2 #include "CMPS_DAGOZ.h"
calmantara186 1:8b6487805a90 3 #include "EncoderDAGOZ.h"
calmantara186 1:8b6487805a90 4 #include "MotorDAGOZ.h"
calmantara186 1:8b6487805a90 5 #include "EncoderMotor.h"
calmantara186 1:8b6487805a90 6 #include "PID.h"
calmantara186 1:8b6487805a90 7 #include "Constanta.h"
calmantara186 2:830d3c808679 8 // #include <ros.h>
calmantara186 2:830d3c808679 9 // #include <rosserial_mbed/HardwareCommandMsg.h>
calmantara186 2:830d3c808679 10 // #include <rosserial_mbed/HardwareStateMsg.h>
calmantara186 1:8b6487805a90 11
calmantara186 1:8b6487805a90 12 /*****************************
calmantara186 1:8b6487805a90 13 ROS node handle
calmantara186 1:8b6487805a90 14 *****************************/
calmantara186 2:830d3c808679 15 // ros::NodeHandle nh;
calmantara186 1:8b6487805a90 16
calmantara186 1:8b6487805a90 17 /*****************************
calmantara186 1:8b6487805a90 18 Pin Declaration
calmantara186 1:8b6487805a90 19 *****************************/
calmantara186 1:8b6487805a90 20 //Encoder pin
calmantara186 2:830d3c808679 21 EncoderDAGOZ locomotionEncL(TIM1); //Locomotion Right Encoder
calmantara186 2:830d3c808679 22 EncoderDAGOZ locomotionEncR(TIM3); //Locomotion Left Encoder
calmantara186 1:8b6487805a90 23 EncoderDAGOZ locomotionEncB(TIM2); //Locomotion Back Encoder
calmantara186 2:830d3c808679 24 EncoderDAGOZ dribblerEncL(TIM4); //Dribbler Right Encoder
calmantara186 2:830d3c808679 25 EncoderDAGOZ dribblerEncR(TIM8); //Dribbler Left Encoder
calmantara186 1:8b6487805a90 26
calmantara186 2:830d3c808679 27 EncoderMotor intEncR(PG_8, PE_0, 537.6, EncoderMotor::X4_ENCODING);
calmantara186 2:830d3c808679 28 EncoderMotor intEncB(PD_10, PG_14, 537.6, EncoderMotor::X4_ENCODING);
calmantara186 2:830d3c808679 29 EncoderMotor intEncL(PF_2, PF_1, 537.6, EncoderMotor::X4_ENCODING);
calmantara186 1:8b6487805a90 30
calmantara186 1:8b6487805a90 31 //Motor pin buat Board Sistem next ver.
calmantara186 1:8b6487805a90 32 MotorDagoz locomotionMotorL(PF_12, PF_8); //Locomotion Right Motor
calmantara186 1:8b6487805a90 33 MotorDagoz locomotionMotorR(PG_1, PF_9); //Locomotion Left Motor
calmantara186 1:8b6487805a90 34 MotorDagoz locomotionMotorB(PF_11, PF_7); //Locomotion Back Motor
calmantara186 1:8b6487805a90 35
calmantara186 1:8b6487805a90 36 MotorDagoz dribblerMotorR(PF_15, PE_6); //Dribbler Right Motor
calmantara186 1:8b6487805a90 37 MotorDagoz dribblerMotorL(PE_15, PE_5); //Dribbler Left Motor
calmantara186 1:8b6487805a90 38
calmantara186 1:8b6487805a90 39
calmantara186 1:8b6487805a90 40 //Serial pin
calmantara186 2:830d3c808679 41 Serial pc(USBTX, USBRX, 115200); //Serial debug
calmantara186 1:8b6487805a90 42 //Compass CMPS12 pin
calmantara186 2:830d3c808679 43 CMPS_DAGOZ compass(PB_9, PB_8, 0xC0); //Compass I2C Communication SDA SCL
calmantara186 1:8b6487805a90 44 DigitalOut compassLed(PC_5); //CMPS error indicator
calmantara186 0:1c22457d4aed 45
calmantara186 1:8b6487805a90 46 //Potensio Pin
calmantara186 2:830d3c808679 47 AnalogIn dribblerPotL(PF_6); //Potensio for Left Dribbler, di board saat ini masih PC_2
calmantara186 2:830d3c808679 48 AnalogIn dribblerPotR(PC_3); //Potensio for Right Dribbler
calmantara186 2:830d3c808679 49 AnalogIn infraRed(PF_3); //Potensio for Kicker, di board saat ini masih PD_4
calmantara186 1:8b6487805a90 50
calmantara186 1:8b6487805a90 51 //LED Pin
calmantara186 1:8b6487805a90 52 DigitalOut ledRed(PG_4); //Red LED
calmantara186 1:8b6487805a90 53 DigitalOut ledGreen(PG_5); //Green LED
calmantara186 1:8b6487805a90 54 DigitalOut ledBlue(PG_6); //Blue LED
calmantara186 2:830d3c808679 55
calmantara186 1:8b6487805a90 56 //Kicker Pin
calmantara186 1:8b6487805a90 57 PwmOut kicker(PB_15); //Kicker pwm effort, di board saat ini masih PC_8
calmantara186 1:8b6487805a90 58 //Stepper Pin
calmantara186 1:8b6487805a90 59 DigitalOut stepperDir(PA_10); //Direction pin for stepper
calmantara186 1:8b6487805a90 60 DigitalOut stepperStep(PF_10); //Step pin for stepper, di board saat ini masih PC_4
calmantara186 1:8b6487805a90 61
calmantara186 1:8b6487805a90 62 /******************************
calmantara186 1:8b6487805a90 63 Publisher-Subscriber
calmantara186 1:8b6487805a90 64 ******************************/
calmantara186 2:830d3c808679 65 // dagozilla_msgs::HardwareStateMsg stateMsg;
calmantara186 2:830d3c808679 66 // ros::Publisher statePub("/hardware/state", &stateMsg);
calmantara186 1:8b6487805a90 67
calmantara186 2:830d3c808679 68 // void commandCallback(const dagozilla_msgs::HardwareCommandMsg& commandMsg);
calmantara186 2:830d3c808679 69 // ros::Subscriber<dagozilla_msgs::HardwareCommandMsg> commandSub("/hardware/command", &commandCallback);
calmantara186 2:830d3c808679 70
calmantara186 2:830d3c808679 71 //PID global object PID, P I D N TS FF MODE
calmantara186 2:830d3c808679 72 PID locomotionPidR(0.142174, 4.898472, 0, 100, 0.02, 0, PID::PID_MODE); //Right locomotion PID
calmantara186 2:830d3c808679 73 PID locomotionPidL(0.110189, 4.861782, 0, 100, 0.02, 0, PID::PID_MODE); //Left locomotion PID
calmantara186 2:830d3c808679 74 PID locomotionPidB(0.142982, 4.840596, 0, 100, 0.02, 0, PID::PID_MODE); //Back locomotion
calmantara186 1:8b6487805a90 75
calmantara186 1:8b6487805a90 76 //thread for RTOS
calmantara186 1:8b6487805a90 77 Thread thread1;
calmantara186 1:8b6487805a90 78 Thread thread2;
calmantara186 1:8b6487805a90 79
calmantara186 1:8b6487805a90 80 //primitive function
calmantara186 1:8b6487805a90 81 void mainProcess();
calmantara186 1:8b6487805a90 82 void getCompass();
calmantara186 1:8b6487805a90 83 void moveLocomotion();
calmantara186 1:8b6487805a90 84 void moveDribbler();
calmantara186 1:8b6487805a90 85 void publishMessage();
calmantara186 1:8b6487805a90 86
calmantara186 2:830d3c808679 87 bool dribbler_state;
calmantara186 2:830d3c808679 88
calmantara186 1:8b6487805a90 89 /*****************************
calmantara186 1:8b6487805a90 90 Main Function
calmantara186 1:8b6487805a90 91 *****************************/
calmantara186 1:8b6487805a90 92
calmantara186 1:8b6487805a90 93 int main()
calmantara186 0:1c22457d4aed 94 {
calmantara186 1:8b6487805a90 95 //init ros advertise and subscribe
calmantara186 2:830d3c808679 96 // nh.initNode();
calmantara186 2:830d3c808679 97 // nh.advertise(statePub);
calmantara186 2:830d3c808679 98 // nh.subscribe(commandSub);
calmantara186 0:1c22457d4aed 99
calmantara186 1:8b6487805a90 100 kicker.period(0.01f);
calmantara186 1:8b6487805a90 101 kicker = 1; //deactivate kicker, active LOW
calmantara186 0:1c22457d4aed 102
calmantara186 1:8b6487805a90 103 thread1.start(mainProcess);
calmantara186 2:830d3c808679 104 thread2.start(getCompass);
calmantara186 1:8b6487805a90 105
calmantara186 0:1c22457d4aed 106 while (true) {
calmantara186 1:8b6487805a90 107 //do nothing
calmantara186 0:1c22457d4aed 108 }
calmantara186 0:1c22457d4aed 109 }
calmantara186 0:1c22457d4aed 110
calmantara186 1:8b6487805a90 111 void mainProcess(){
calmantara186 1:8b6487805a90 112 float cur_pot_L0 = ((float)dribblerPotL.read()*100.0f);
calmantara186 1:8b6487805a90 113 float cur_pot_R0 = ((float)dribblerPotR.read()*100.0f);
calmantara186 1:8b6487805a90 114 Thread::wait(1000);
calmantara186 1:8b6487805a90 115
calmantara186 2:830d3c808679 116 float rotInR, rotInL, rotInB;
calmantara186 1:8b6487805a90 117
calmantara186 1:8b6487805a90 118 while(1){
calmantara186 2:830d3c808679 119 // nh.spinOnce();
calmantara186 1:8b6487805a90 120 cur_pot_L = ((float)dribblerPotL.read()*100.0f-cur_pot_L0);
calmantara186 2:830d3c808679 121 cur_pot_R = -((float)dribblerPotR.read()*100.0f-cur_pot_R0);
calmantara186 1:8b6487805a90 122 //ball distance from IR
calmantara186 2:830d3c808679 123 distance = infraRed.read();
calmantara186 1:8b6487805a90 124 //read encoder
calmantara186 2:830d3c808679 125 cur_locomotion_R = -locomotionEncR.GetCounter(1)/4000.0;
calmantara186 2:830d3c808679 126 cur_locomotion_L = -locomotionEncL.GetCounter(1)/4000.0;
calmantara186 2:830d3c808679 127 cur_locomotion_B = -locomotionEncB.GetCounter(1)/4000.0;
calmantara186 1:8b6487805a90 128
calmantara186 2:830d3c808679 129 rotInB = intEncR.getRevolutions();
calmantara186 2:830d3c808679 130 rotInR = intEncL.getRevolutions();
calmantara186 2:830d3c808679 131 rotInL = intEncB.getRevolutions();
calmantara186 1:8b6487805a90 132
calmantara186 2:830d3c808679 133 cur_dribbler_R = dribblerEncR.GetCounter(1)/537.6;
calmantara186 2:830d3c808679 134 cur_dribbler_L = -dribblerEncL.GetCounter(1)/537.6;
calmantara186 1:8b6487805a90 135 // Correct encoder unit
calmantara186 1:8b6487805a90 136 locomotion_R_rot = cur_locomotion_R * LOCOMOTIONWHEEL * 2 * PI; // in m
calmantara186 1:8b6487805a90 137 locomotion_L_rot = cur_locomotion_L * LOCOMOTIONWHEEL * 2 * PI; // in m
calmantara186 1:8b6487805a90 138 locomotion_B_rot = cur_locomotion_B * LOCOMOTIONWHEEL * 2 * PI; // in m
calmantara186 1:8b6487805a90 139 dribbler_R_rot = cur_dribbler_L * DRIBBLERWHEEL * 2 * PI; // in m
calmantara186 1:8b6487805a90 140 dribbler_L_rot = cur_dribbler_R * DRIBBLERWHEEL * 2 * PI; // in m
calmantara186 1:8b6487805a90 141 // Calculate acutal velocity
calmantara186 2:830d3c808679 142 locomotion_R_vel = (rotInR*2*PI*0.05) / 0.02;
calmantara186 2:830d3c808679 143 locomotion_L_vel = (rotInL*2*PI*0.05) / 0.02;
calmantara186 2:830d3c808679 144 locomotion_B_vel = (rotInB*2*PI*0.05) / 0.02;
calmantara186 2:830d3c808679 145 dribbler_R_vel = dribbler_R_rot / 0.02;
calmantara186 2:830d3c808679 146 dribbler_L_vel = dribbler_L_rot / 0.02;
calmantara186 1:8b6487805a90 147
calmantara186 2:830d3c808679 148 // kicker conditional
calmantara186 1:8b6487805a90 149 // if(kick_power_target > 0) {
calmantara186 1:8b6487805a90 150 // kicker.write(1-kick_power_target); //Active LOW
calmantara186 1:8b6487805a90 151 // Thread::wait(10);
calmantara186 1:8b6487805a90 152 // kicker = 1;
calmantara186 1:8b6487805a90 153 // kick_power_target = 0;
calmantara186 1:8b6487805a90 154 // }
calmantara186 2:830d3c808679 155 //moveLocomotion();
calmantara186 2:830d3c808679 156 moveDribbler(); //active dribbler state
calmantara186 1:8b6487805a90 157
calmantara186 2:830d3c808679 158 // publishMessage();
calmantara186 2:830d3c808679 159 // nh.spinOnce();
calmantara186 2:830d3c808679 160 Thread::wait(20);
calmantara186 0:1c22457d4aed 161 }
calmantara186 0:1c22457d4aed 162 }
calmantara186 1:8b6487805a90 163
calmantara186 1:8b6487805a90 164 void getCompass(){
calmantara186 1:8b6487805a90 165 float theta0 = compass.readBearing()/10.0;
calmantara186 1:8b6487805a90 166 Thread::wait(1000);
calmantara186 2:830d3c808679 167 theta0 = compass.readBearing()/10.0;
calmantara186 1:8b6487805a90 168 compassLed = 0;
calmantara186 1:8b6487805a90 169 while(1){
calmantara186 1:8b6487805a90 170 float theta_temp = (compass.readBearing()/10.0) - theta0;
calmantara186 1:8b6487805a90 171 if(theta_temp > 180.0 && theta_temp <= 360.0)
calmantara186 1:8b6487805a90 172 theta_com = ((compass.readBearing()/10.0) - 360.0 - theta0)/-RADTODEG;
calmantara186 1:8b6487805a90 173 else if(theta_temp < -180.0 && theta_temp >= -360.0)
calmantara186 1:8b6487805a90 174 theta_com = ((compass.readBearing()/10.0) + 360.0 - theta0)/-RADTODEG;
calmantara186 1:8b6487805a90 175 else
calmantara186 1:8b6487805a90 176 theta_com = ((compass.readBearing()/10.0) - theta0)/-RADTODEG;
calmantara186 1:8b6487805a90 177
calmantara186 1:8b6487805a90 178 if(theta != 0) compassLed = 1;
calmantara186 1:8b6487805a90 179 else compassLed = 0;
calmantara186 1:8b6487805a90 180
calmantara186 1:8b6487805a90 181 theta = theta_com;
calmantara186 2:830d3c808679 182
calmantara186 2:830d3c808679 183 if(theta != 0) compassLed = 1;
calmantara186 2:830d3c808679 184 else compassLed = 0;
calmantara186 2:830d3c808679 185
calmantara186 2:830d3c808679 186 Thread::wait(60);
calmantara186 1:8b6487805a90 187 }
calmantara186 1:8b6487805a90 188 }
calmantara186 1:8b6487805a90 189
calmantara186 1:8b6487805a90 190 void moveLocomotion(){
calmantara186 1:8b6487805a90 191 // Calculate motor pwm
calmantara186 1:8b6487805a90 192 locomotion_R_target_rate = locomotionPidR.createpwm(locomotion_R_vtarget, locomotion_R_vel);
calmantara186 1:8b6487805a90 193 locomotion_L_target_rate = locomotionPidL.createpwm(locomotion_L_vtarget, locomotion_L_vel);
calmantara186 1:8b6487805a90 194 locomotion_B_target_rate = locomotionPidB.createpwm(locomotion_B_vtarget, locomotion_B_vel);
calmantara186 2:830d3c808679 195
calmantara186 1:8b6487805a90 196 //Motor pwm
calmantara186 1:8b6487805a90 197 locomotionMotorR.setpwm(locomotion_R_target_rate);
calmantara186 1:8b6487805a90 198 locomotionMotorL.setpwm(locomotion_L_target_rate);
calmantara186 1:8b6487805a90 199 locomotionMotorB.setpwm(locomotion_B_target_rate);
calmantara186 1:8b6487805a90 200 }
calmantara186 1:8b6487805a90 201
calmantara186 1:8b6487805a90 202 void moveDribbler(){
calmantara186 2:830d3c808679 203 float RKXFF, LKXFF; //Dribbler x velocity feedforward
calmantara186 2:830d3c808679 204 float RKYFF, LKYFF; //Dribbler y velocity feedforward
calmantara186 2:830d3c808679 205
calmantara186 1:8b6487805a90 206 if (vx <= 0){
calmantara186 2:830d3c808679 207 RKXFF = 0.9; LKXFF = 0.3;
calmantara186 2:830d3c808679 208 } else{
calmantara186 2:830d3c808679 209 RKXFF = 0.3; LKXFF = 0.9;
calmantara186 2:830d3c808679 210 }
calmantara186 2:830d3c808679 211
calmantara186 2:830d3c808679 212 if (vy > 0){
calmantara186 2:830d3c808679 213 RKYFF = 1; LKYFF = 1;
calmantara186 1:8b6487805a90 214 } else{
calmantara186 2:830d3c808679 215 RKYFF = 1.3; LKYFF = 1.3;
calmantara186 1:8b6487805a90 216 }
calmantara186 2:830d3c808679 217
calmantara186 2:830d3c808679 218 //Compute value//
calmantara186 2:830d3c808679 219 vx = (locomotion_B_vel*2/3) + (-locomotion_R_vel/3) + (-locomotion_L_vel/3);
calmantara186 2:830d3c808679 220 vy = (locomotion_R_vel*2/3) + (-locomotion_L_vel*2/3);
calmantara186 2:830d3c808679 221 w = (locomotion_B_vel+locomotion_L_vel+locomotion_R_vel)/(3*0.21);
calmantara186 1:8b6487805a90 222 //Calculate Feedback and FeedForward
calmantara186 1:8b6487805a90 223 //need correction for +- in each robot velocity
calmantara186 2:830d3c808679 224 float dribbler_R_vel_target = (LTARGET - cur_pot_L)*LK1FB+(fabs(vx*LKXFF) - (vy*LKYFF) + fabs(w*LKTFF));
calmantara186 2:830d3c808679 225 float dribbler_L_vel_target = (RTARGET - cur_pot_R)*RK1FB+(fabs(vx*RKXFF) - (vy*RKYFF) + fabs(w*RKTFF));
calmantara186 2:830d3c808679 226 dribbler_L_target_rate = ((dribbler_L_vel_target - dribbler_L_vel)*LK2FB)+(LKVFF*(dribbler_L_vel_target));
calmantara186 2:830d3c808679 227 dribbler_R_target_rate = ((dribbler_R_vel_target - dribbler_R_vel)*RK2FB)+(RKVFF*(dribbler_R_vel_target));
calmantara186 2:830d3c808679 228
calmantara186 1:8b6487805a90 229 if(dribbler_R_target_rate>1.0) dribbler_R_target_rate = 1.0;
calmantara186 1:8b6487805a90 230 if(dribbler_L_target_rate>1.0) dribbler_L_target_rate = 1.0;
calmantara186 1:8b6487805a90 231
calmantara186 2:830d3c808679 232 if(abs(LTARGET - cur_pot_L)<=5.0) dribbler_R_target_rate = 0.0;
calmantara186 2:830d3c808679 233 if(abs(RTARGET - cur_pot_R)<=5.0) dribbler_L_target_rate = 0.0;
calmantara186 2:830d3c808679 234 /*pc.printf("targetka=%.2f targetki=%.2f potka=%.2f potki=%.2f xka=%.2f xki=%.2f yka=%.2f yki=%.2f wka=%.2f wki=%.2f\n",
calmantara186 2:830d3c808679 235 dribbler_R_vel_target, dribbler_L_vel_target,
calmantara186 2:830d3c808679 236 (RTARGET - cur_pot_R)*RK1FB, (LTARGET - cur_pot_L)*LK1FB,
calmantara186 2:830d3c808679 237 fabs(vx*RKXFF), fabs(vx*LKXFF),
calmantara186 2:830d3c808679 238 (-vy*RKYFF), (-vy*LKYFF),
calmantara186 2:830d3c808679 239 fabs(w*RKTFF), fabs(w*LKTFF));*/
calmantara186 2:830d3c808679 240
calmantara186 2:830d3c808679 241 if(dribbler_state){
calmantara186 2:830d3c808679 242 dribblerMotorL.setpwm(dribbler_L_target_rate);
calmantara186 2:830d3c808679 243 dribblerMotorR.setpwm(dribbler_R_target_rate);
calmantara186 2:830d3c808679 244 } else {
calmantara186 2:830d3c808679 245 dribblerMotorL.setpwm(0);
calmantara186 2:830d3c808679 246 dribblerMotorR.setpwm(0);
calmantara186 2:830d3c808679 247 }
calmantara186 1:8b6487805a90 248 dribblerMotorL.setpwm(dribbler_L_target_rate);
calmantara186 1:8b6487805a90 249 dribblerMotorR.setpwm(dribbler_R_target_rate);
calmantara186 2:830d3c808679 250
calmantara186 1:8b6487805a90 251 }
calmantara186 1:8b6487805a90 252
calmantara186 2:830d3c808679 253 // void publishMessage(){
calmantara186 2:830d3c808679 254 // // Publish position data
calmantara186 2:830d3c808679 255 // stateMsg.base_wheel_right_position = locomotion_R_rot; // in m
calmantara186 2:830d3c808679 256 // stateMsg.base_wheel_back_position = locomotion_B_rot; // in m
calmantara186 2:830d3c808679 257 // stateMsg.base_wheel_left_position = locomotion_L_rot; // in m
calmantara186 2:830d3c808679 258 // // Publish velocity data
calmantara186 2:830d3c808679 259 // stateMsg.base_wheel_right_velocity = locomotion_R_vel; // in m/s
calmantara186 2:830d3c808679 260 // stateMsg.base_wheel_back_velocity = locomotion_B_vel; // in m/s
calmantara186 2:830d3c808679 261 // stateMsg.base_wheel_left_velocity = locomotion_L_vel; // in m/s
calmantara186 2:830d3c808679 262 // // Publish distance data
calmantara186 2:830d3c808679 263 // stateMsg.dribbler_ir_distance = distance; // in analog value
calmantara186 2:830d3c808679 264 // stateMsg.compass_angle = theta;
calmantara186 2:830d3c808679 265 // stateMsg.dribbler_active = dribbler_state;
calmantara186 2:830d3c808679 266 // statePub.publish(&stateMsg);
calmantara186 2:830d3c808679 267 // }
calmantara186 1:8b6487805a90 268
calmantara186 2:830d3c808679 269 // void commandCallback(const dagozilla_msgs::HardwareCommandMsg& commandMsg) {
calmantara186 2:830d3c808679 270 // locomotion_R_vtarget = commandMsg.base_wheel_right_velocity;
calmantara186 2:830d3c808679 271 // locomotion_B_vtarget = commandMsg.base_wheel_back_velocity;
calmantara186 2:830d3c808679 272 // locomotion_L_vtarget = commandMsg.base_wheel_left_velocity;
calmantara186 2:830d3c808679 273 // kick_power_target = commandMsg.kicker_effort;
calmantara186 2:830d3c808679 274 // dribbler_state = commandMsg.dribbler_active;
calmantara186 2:830d3c808679 275 // }